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I.    INTRODUCTION 

It  is  estimated  that  nearly  half  of  the  Earth's  land  surface  is  inaccessible  to 
wheeled  and  tracked  vehicles  [Ref.  l].  Yet  almost  all  of  this  same  area  can  be 
successfully  traversed  by  animals  and  man.  This  great  difference  in  mobility  has 
motivated  research  into  the  creation  of  a  practical  legged  vehicle  or 
walking  machine  . 

The  advantages  of  legged  locomotion  can  largely  be  attributed  to  the 
flexibility  offered  in  leg  placement  and  support.  Wheeled  vehicles,  and  to  a  lesser 
extent  tracked  vehicles,  are  confined  to  a  more  or  less  continuous,  relatively  flat 
and  obstruction  free  paths  along  the  ground.  The  leg's  flexibility  allows  the 
utilization  of  discontinuous  support  regions  on  the  ground  and  the  adaptation  to 
terrain  slope.  A  legged  vehicle  may  potentially  use  obstructions  for  support  as  it 
climbs  over  those  obstacles  which  it  decides  to  not  simply  ignore. 

A  second  advantage  of  legs  involves  the  means  of  obtaining  traction  in  soft 
soil.  A  wheel  or  track  creates  a  depression  or  rut  from  which  it  must  continually 
work  to  climb  out.  Slippage  causes  the  wheel  or  track  spin,  possibly  digging  a 
deeper  hole-  A  leg,  however,  may  be  lifted  vertically  out  of  its  depression, 
minimizing  the  work  required.  In  addition,  any  back  slip  caused  by  the  vehicle 
stepping  pushes  up  soil  behind  the  foot  and  improves  traction.  [Ref.  2] 


The  combination  of  flexible  coordination  and  increased  traction  provides  a 
potential  for  greater  speed  and  less  power  consumption  while  operating  over  rough 
and  otherwise  unsuitable  terrain.  Other  advantages  of  legged  locomotion  include 
possible  improved  comfort  in  ride  due  to  the  adaptive  nature  of  legged  support  on 
uneven  terrain,  the  ability  to  test  soil  conditions  prior  to  placement  of  weight  on 
the  legs,  and  the  relatively  small  footprint  left  in  the  soil.  The  latter  may  prove 
especially  important  for  agricultural  work,  where  the  disturbance  of  crops  is  to  be 
minimized,  or  for  military  vehicles  navigating  areas  suspected  of  containing 
landmines. 

A.    GOALS 

The  purpose  of  this  study  is  to  explore  a  new  type  of  gait  and  steering 
algorithm  for  the  use  of  legged  walking  machines.  The  gait  is  a  particular  type  of 
tripod  gait,  which  can  be  considered  as  an  extension  of  the  temporal  follow-the- 
leader  gait  [Ref.  3].  into  the  spatial  domain.  The  steering  algorithm  to  be 
investigated  along  with  this  style  of  gait  borrows  from  the  concept  of  driving  a 
wheeled  tractor-trailer  vehicle.  It  is  believed  that  this  steering  algorithm  may  be 
particularly  well  suited  for  the  fixed  foothold  position  requirements  of  follow-the- 
leader  gaits. 

The  machine  chosen  as  a  physical  reference  for  the  study  is  the  Adaptive 
Suspension  Vehicle  (ASV).    This  is  a  self-contained,  six-legged  vehicle  currently 
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being  evaluated  at  the  Ohio  State  University  for  rough-terrain  locomotion.  The 
ASV  is  a  Defense  Advanced  Research  Projects  Agency  (DARPA)  proof  of  concept 
project. 

A  secondary  goal  is  to  develop  a  simulation  model  with  which  to  study 
walking  gaits  and  control  algorithms  in  general  for  the  ASV.  This  model  is 
developed  along  the  general  lines  of  the  simulation  previously  presented  by  Lee 
[Ref.  4],  incorporating  several  of  his  model's  features,  including  omni-directional 
control,  foot  movement,  and  body  attitude  and  altitude  regulation  algorithms.  In 
addition,  this  simulation  is  to  have  the  features  of  operation  in  either  the  new 
follow-the-leader  tripod  gait  mode  or  in  Lee"s  "forward  wave"  tripod  gait  mode, 
an  enhancement  of  realism  with  a  detailed  color  graphics  display,  and  a  menu 
system  controlled  with  a  single  mouse  button. 


B.    ORGANIZATION 

Chapter  II  provides  a  brief  overview  of  the  previous  work  relating  to  this 
study.  It  includes  a  discussion  of  state  of  the  art  legged  vehicles,  tripod  follow- 
the-leader  gaits,  tripod  gaits,  stability  and  simulation  displays. 

A  detailed  discussion  of  the  ASV  simulation  problem  is  presented  in  Chapter 
III.  This  chapter  covers  the  configuration  of  the  vehicle,  the  gait  and  steering 
algorithms,  the  simplifications  assumed  in  the  construction  of  the  model,  and  the 
kinematics   involved   in  making  the  ASV  model  walk.     The  final  section  in  this 
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chapter  describes  the  IRIS-2400  simulation  hardware  and  software  on  which  the 
model  was  developed. 

The  simulation  program's  operation  and  functions  are  presented  in  Chapter 
IV.  This  includes  a  complete  description  of  the  operation  of  the  program  controls 
and  display  features.  This  is  followed  by  a  discussion  of  the  means  by  which 
graphics  are  programmed  on  the  IRIS,  and  by  a  description  of  the  organization 
and  flow  of  the  program  and  its  modules. 

Chapter  V  is  a  review  of  the  performance  of  the  simulation.  It  includes  a 
brief  subjective  view  on  the  feel  of  driving  in  the  two  modes. 

The  final  chapter  summarizes  the  contributions  of  this  study.  It  also  contains 
comments  on  possible  directions  for  future  research.  The  program  code  is  listed  in 
the  appendix. 
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II.    SURVEY  OF  PREVIOUS  WORK 

A.    INTRODUCTION 

The  last  quarter  of  a  century  has  witnessed  intense  efforts  to  build  machines 
that  walk.  Difficulties  facing  researchers  include  the  problems  of  controlling  the 
many  degrees  of  freedom  necessary  in  a  maneuverable  leg.  maintaining  vehicle 
stability,  creating  energy  efficient  motion,  and  adapting  the  walking  motions  to 
unstructured  terrain.  With  the  advent  of  compact  computer  technology  and 
computer-aided  simulation  and  design,  serious  progress  is  now  being  made  in 
overcoming  these  problems.  [Ref.  5] 

Several  promising  working  designs  have  emerged  in  the  last  ten  years.  Some 
of  the  most  prominent  include  the  Perambulating  Vehicle  II  (PVII)  at  the  Tokyo 
Institute  of  Technology,  the  Carnegie-Mellon  University  hexapod.  the  Odetics  Inc. 
ODEX  I,  and  the  Adaptive  Suspension  Vehicle  (ASV)  developed  at  the  Ohio 
State  University. 

The  PVII  is  a  light-weight  laboratory  model  quadruped,  developed  in  1980. 
It  features  one  of  the  first  pantograph  leg  constructions  designed  specifically  to 
provide  simplified  leg  coordination  and  energy  efficient  walking.  Using  tactile  foot 
sensors  and  a  microcomputer  mounted  near  the  vehicle,  the  PVII  is  able  to  probe 
for  footholds  and  maneuver  over  obstacles.    [Ref.  6] 
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The  hexapod  developed  at  the  Carnegie-Mellon  University  in  1982  is  a  self- 
contained  walking  machine  large  enough  to  earn-  its  operator.  It  uses  a  gasoline 
engine  to  provide  power  to  the  legs  via  a  set  of  hydraulic  actuators.  The 
movements  of  the  individual  legs  are  controlled  by  a  series  of  passive  hydraulic 
circuits.  A  built-in  microprocessor  interprets  the  driver's  commands  and  specifies 
the  correct  series  of  leg  movement  patterns  to  be  used.  This  arrangement  frees 
the  single  microprocessor  from  the  need  to  compute  each  foot  trajectory.  [Ref.  7] 

The  ODEX  I  is  a  commercial  design  introduced  in  1983  [Ref.  8].  An 
improved  version,  sometimes  referred  to  as  ODEX  II.  is  being  developed  for  near- 
term  use  in  nuclear  power  plants  [Ref.  9].  The  ODEX  series  makes  use  of  a 
unique  circular  arrangement  of  six  planar  pantograph  legs  which  allow  the 
vehicles  to  adjust  their  profile  for  negotiation  of  narrow  passages.  The  ODEX 
walking  machines  are  directed  through  a  radio  or  fiber-optic  link  from  the 
operator  to  an  on-board  supervisory-level  microprocessor.  Each  leg  is  controlled 
by  a  dedicated  lower-level  microprocessor  which  receives  instructions  from  the 
supervisory  level  microprocessor.  The  new  ODEX  hexapod  is  also  being  equipped 
with  a  center-mounted  arm  for  remote  manipulation  of  objects,  such  as  valves,  in 
hazardous  environments.  [Ref.  8] 

The  Adaptive  Suspension  Vehicle  (Figure  2.1),  currently  being  tested  at  Ohio 

State   University,   is  the   first  computer-coordinated   legged  vehicle  designed  and 

built  for  operation  on  natural  terrain  [Ref.  10] .    This  hexapod  walking  machine  is 

completely  self-contained,  and  is  capable  of  carrying  the  driver,  a  500  lb.  internal 
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payload.  computer  and  control  circuitry,  and  power  system,  in  an  outdoor 
environment.  The  ASV  is  the  vehicle  modeled  in  this  study.  A  more  detailed 
description  of  the  ASV  follows  in  Section  3.B. 

The  remaining  sections  of  this  chapter  concern  gaits  used  by  walking 
machines,  vehicle  steering,  the  walking  machine  stability  problem,  and  graphical 
representation  of  the  vehicle's  motion.  The  gait  and  stability  sections  are  oriented 
towards  six-legged  vehicles  such  as  the  ASV. 

B.    GAIT  SELECTION 

1.      Definitions 

A  gait  is  a  mode  of  locomotion  for  a  vehicle  or  animal  distinguished  by  a 

specific    pattern    of   lifting    and    placing    of   the    feet.     Gaits    in    general   may    be 

described  using  the  event  sequence  notation  introduced  by  McGhee  and  Jain  [Ref. 

11].    The  integer  i  in  such  a  sequence  corresponds  to  the  event  of  placing  foot  i  on 

the  ground.     The   lifting  of  the  same  foot    is  represented  by  the  integer   /'  +  n. 

where  n  equals  the  number  of  legs.    For  the  ASV,  legs  are  numbered  on  the  left 

side  (1.  3.  5)  from  the  front  to  the  rear,  and  on  the  right  side  (2.  4.  6)  in  the  same 

order. 

A  periodic  gait   is  one  that   repeats  the  lifting  and  placing  pattern,  and 

thus    is    represented    by    one    cycle    of   events.      A    periodic    gait    is    said    to    be 

nonsingular   if   no    two   of   its    events    occur    simultaneously.     McGhee    [Ref.    12] 

demonstrated  the  existence  of  39.916.800  possible  nonsingular  periodic  hexapod 
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gaits.  The  total  number  of  possible  hexapod  gaits  is  a  much  larger  and  unknown 
number  [Ref.  5].  This  makes  the  selection  of  an  optimum  gait  a  very  difficult 
problem.  However,  this  thesis  is  concerned  with  a  single  type  of  gait  sequence, 
the  tripod  sequence.  These  are  singular  gaits,  in  that  more  than  one  leg  is  placed 
at  a  given  instant  [Ref.  3j. 

A  periodic  gait  is  considered  symmetrical  when  the  stepping  pattern  on 
one  side  of  the  body  is  identical  to  that  on  the  opposite  side  and  separated  in  time 
by  exactly  one-half  of  the  gait  period  [Ref.  12].  Symmetry  tends  to  simplify  the 
required  leg  coordination  algorithms. 

The  pitch  of  a  gait  is  the  distance  between  footholds,  measured  in  body 
lengths  (defined  as  the  distance  between  the  front  and  rear  leg  reference 
positions).  Leg  stroke  is  the  linear  distance  the  foot  travels  with  respect  to  the 
body  when  occupying  a  particular  foothold.  Leg  stroke  is  also  expressed  in  terms 
of  body  lengths. 

2.     Follow-the- Leader  Gaits 

A  follow  -the  -leader  (FTL)  gait  is  one  in  which  the  middle  and  rear  legs 
on  each  side  of  the  body  step  in  the  foothold  locations  previously  occupied  by  the 
leading  legs  [Ref.  13].  Creeping  FTL  gaits  (in  which  at  most  one  leg  is  in  the  air 
at  any  time  [Ref.  14]).  were  first  studied  by  Ozguner,  Tsai,  and  McGhee  [Ref.  3]. 
Losing  a  temporal  framework,  they  narrowed  the  number  of  possible  FTL  creeping 
gaits  to  30,  of  which  they  found  five  to  be  symmetrically  realizable. 
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Expanding  to  a  spatial  reference  frame  greatly  increases  the  number  of 
gaits  in  this  category.  A  tripod  creeping  gait  can  be  defined  as  one  in  which  the 
legs  are  placed  in  alternating  groups  of  three,  with  each  group  forming  a  tripod  of 
support.  In  the  case  of  the  ASV,  the  two  possible  tripods  are  the  leg  sets  (1  4  5) 
and  (2  3  6). 

The  possible  distinct  tripod  creeping  gaits  can  be  ennumerated  using  an 
approach  similar  to  that  in  Ozguner  et  al.  [Ref.  3].  Choosing  the  placement  of  leg 
1  as  a  reference,  evidently  there  are  two  possibilities  for  the  relative  ordering  of 
legs  4  and  5.  and  three  possible  locations  in  each  sequence  for  the  insertion  of  the 
alternate  group  of  legs.  Furthermore,  the  placing  of  the  alternate  leg  group  (2  3 
6)  can  be  accomplished  in  six  distinct  ways.  Table  2.1  lists  the  36  possible 
nonsingular  placing  sequences. 

It  might  first  appear  strange  that  the  sequence  (  12  3  6  4  5)  is  included 
in  Table  2.1.  However  taking  two  periods  together,  the  sequence  becomes 
(123645123645).  which  clearly  shows  that  the  placement  of  the  legs 
occur  in  alternating  groups  of  three. 

Comparing  the  entries  in  Table  2.1  to  those  in  the  table  of  Ozguner  et  al., 
one  can  see  that  none  of  these  sequences  are  listed  in  the  latter  work.  This  is 
because  the  sequences  here  are  not  temporally  follow-the-leader.  Yet  they  all  are 
spatial  FTL  gaits.  This  can  be  seen  from  the  gait  kinematics  of  the  example 
shown  in  Figure  2.2. 
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TABLE  2.1. 

PLACING  SEQUENCES  FOR  TRIPOD  CREEPING  FTL  GAITS]1 

Gait 

Placing 

Tripod  2 

Tripod  1 

Tripod  2 

Number 

Sequence 

Insertion 
Position 

Subsequence 

Subsequence 

1 

123645 

145 

236 

2 

126345 

145 

263 

3 

132645 

145 

326 

4 

136245 

145 

362 

5 

162345 

145 

623 

6 

163245 

145 

632 

7 

123654 

154 

236 

8 

126354 

154 

263 

9 

132654 

154 

326 

10 

136254 

154 

362 

11 

162354 

154 

623 

12 

163254 

154 

632 

13 

142365 

2 

145 

236 

14 

142635 

2 

145 

263 

15 

143265 

2 

145 

326 

16 

143625 

2 

145 

362 

17 

146235 

2 

145 

623 

18 

146325 

2 

145 

632 

19 

152364 

2 

154 

236 

20 

152634 

2 

154 

263 

21 

153264 

2 

154 

326 

22 

153624 

2 

154 

362 

23 

156234 

2 

154 

623 

24 

156324 

2 

154 

632 

25 

145236 

3 

145 

236 

26 

145263 

3 

145 

263 

27 

145326 

3 

145 

326 

28 

145362 

3 

145 

362 

39 

145623 

3 

145 

623 

30 

145632 

3 

145 

632 

31 

154236 

3 

154 

236 

32 

154263 

3 

154 

263 

154326 

3 

154 

326 

34 

154362 

3 

154 

362 

35 

154623 

3 

154 

623 

36 

154632 

3 

154 

632 
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Figure  2.2   Sequence  of  Stepping  for  a  Tripod  FTL 
with  Pitch  of  1/3  and  Continuous  Body  Motion 
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It  is  possible  to  alternate  body  motion  with  leg  placement  in  the  tripod 
gait  (Figure  2.3).  This  yields  a  pattern  of  movement  that  is  compatible  with  the 
general  notion  of  a  creeping  gait  [Ref.  3].  It  should  be  noted,  however,  that  while 
such  a  strategy  may  improve  the  static  stability  of  the  gait  [Ref.  3],  the 
intermittent  body  motion  increases  the  leg  stroke  by  a  factor  of  two,  which 
greatly  increases  the  required  working  volume  of  the  legs.  For  this  reason,  and 
also  because  intermittent  body  motion  slows  the  average  vehicle  forward  speed, 
only  the  continuous  body  motion  alternative  will  be  considered  further  in  this 
thesis. 

3.      Singular  Tripod  Gaits 

Tripod  gaits  have  proved  to  provide  a  good  compromise  between 
stability,  maneuverability,  and  ease  of  control  for  the  Ohio  State  University 
Hexapod.  the  ODEX  I.  and  the  ASV.  For  this  reason  tripod  gaits  were  chosen  for 
this  simulation  study. 

It  can  be  seen  that  a  tripod  gait  is  actually  a  special  limiting  case  of  a 
creeping  gait,  where  the  time  between  the  placement  of  individual  legs  within  a 
tripod  grouping  approaches  zero.  Of  the  very  large  (unknown)  number  of  gait 
sequences  possible,  only  one  can  be  classified  as  a  singular  tripod  gait  sequence. 
All  differences  among  varieties  of  tripod  gaits  are  therefore  a  function  of 
kinematics  onlv. 
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Figure  2.3   Sequence  of  Stepping  for  a  Tripod  FTL  with 
Pitch  of  1/3  and  Alternating  Body  and  Leg  Motion 
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The  most  frequently  used  tripod  gait  is  the  limiting  form  of  the 
forward  wave  gait,  where  the  duty  cycle1,  0,  approaches  1/2  [Ref.  4,5].  This 
study  introduces  the  singular  FTL  tripod  gait.  Both  of  these  gaits  are 
implemented  in  the  walking  algorithms  of  this  simulation  and  are  described 
further  in  Chapter  III. 

It  is  interesting  to  note  that  potentially  the  fastest  forward  wave  tripod 
gait  for  the  ASV  is  an  FTL  tripod  gait  with  a  pitch  of  one  (Fig.  2.4).  This  of 
course  can  only  be  considered  a  true  FTL  gait  if  the  feet  are  assumed  to  be 
dimensionless.  In  order  to  prevent  the  legs  from  interfering  with  one  another,  the 
duty  cycle  might  be  made  slightly  less  than  1/2.  This  would  momentarily  leave 
the  vehicle  with  no  supporting  legs  in  contact  with  the  ground.  It  would  also 
have  the  disadvantage  of  not  providing  sufficient  time  for  possible  foothold 
searches  by  the  leading  legs. 

C.    STEERING 

There  are  several  different  approaches  to  steering  currently  used  by  ground 
vehicles.  The  most  familiar  method  is  articulated,  or  automotive  style  steering 
[Ref.  15].  With  a  steering  wheel,  accelerator  and  brake,  the  driver  of  an 
automobile  can  directly  control  the  vehicle's  turning  radius  and  forward  velocity. 


The  duty  cycle  is  the  fraction  of  the  leg  cycle  used  for  supporting  the  body. 
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Figure  2.4   Sequence  of  Stepping  for  a  Tripod  FTL 
with  Pitch  of  1  and  Continuous  Body  Motion 
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Tracked  vehicles,  on  the  other  hand,  most  frequently  utilize  skid  steering.  By 
operating  the  sets  of  tracks  at  differing  rates,  the  driver  controls  the  turning  rate 
and  forward  velocity  of  the  vehicle. 

Tractor-trailers  use  still  another  type  of  steering.  Here  the  driver  steers  far 
forward  of  the  vehicle's  center  of  gravity.  The  trailer  follows  along  in  the  path  of 
the  cab.  with  the  steering  of  its  center  of  gravity  lagging  behind  the  steering  of 
the  cab.  Furthermore,  since  the  trailer's  wheel  axle  orientation  constrains  its 
motion,  the  trailer  is  restricted  to  a  larger  turning  radius  than  the  cab  is  capable 
of  steering. 

Specially  designed  wheeled  vehicles  may  use  omni  -  directional  steering  [Ref. 
16] .  This  rarely  used  method  allows  the  driver  to  specify  turning  rate  and 
velocity  in  any  horizontal  direction. 

Legged  vehicles  have  historically  used  similar  steering  approaches.  McGhee 
and  Iswandhi  [Ref.  17],  introduced  a  two-axis  joystick  control,  analogous  to 
articulated  steering,  in  which  one  axis  controlled  the  turning  radius  and  the  other 
controlled  forward  velocity.  Orin  [Ref.  18],  applied  three-axis  joystick  control  to 
the  Ohio  State  University  Hexapod,  a  small  laboratory  scale  walking  vehicle. 
This  allowed  forward,  lateral  and  rotational  velocities  to  be  specified  by  the 
driver,  providing  steering  control  much  like  that  experienced  in  a  helicopter.  The 
current  ASV  uses  a  similar  three-axis  joystick  control. 

Tractor-trailer  style  steering  has  not  yet  been  applied  to  walking  vehicles. 
This  approach,  which  will  be  developed  in  this  thesis,  should  give  improved  two- 


axis  control  to  the  driver  for  moving  through  areas  of  restricted  maneuverability. 
The  driver  need  only  be  concerned  with  maneuvering  the  front  end  of  the  vehicle. 
The  body  of  the  vehicle  will  follow  along  the  proven  path  established  by  the 
footholds  used  by  the  front  pair  of  legs. 

D.    STABILITY 

The  problem  of  vehicle  balance  is  a  vital  concern  for  walking  machines  and 
has  been  a  focus  of  many  studies.  Legged  vehicles  may  maintain  their  stability 
using  one  of  two  methods,  static  balancing  [Ref.  19j  or  dynamic  balancing  [Ref. 
20]. 

Static  stability  is  attained  by  maintaining  the  vertical  projection  of  the 
vehicle's  center  of  gravity  within  the  polygon  defined  by  the  supporting  legs  [Ref. 
5].  This  method  is  conceptually  simple.  It  is.  however,  only  valid  for  stationary 
or  slow  moving  vehicles,  as  it  neglects  the  effect^  of  inertia  on  stability. 

Dynamic  balancing  is  a  complex  process  which  places  fewer  restrictions  on 
vehicle  velocity.  The  vehicle  may  be  allowed  to  momentarily  move  into  a 
statically  unstable  configuration,  so  long  as.  over  time,  an  adequate  base  of 
support  is  provided  [Ref.  20].  This  is  the  mode  of  balancing  normally  used  by 
man  and  most  vertebrate  animals.  It  remains  an  extremely  complex  process, 
however,  which  is  difficult  to  reproduce  with  legged  vehicles. 

This   model   uses   only   the   static   criteria   for   stability.     Having   the   vehicle 

limited    to    reasonable   velocities    and    the    six    legs    placed    in    alternating   tripod 
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support  patterns  ensures  a  high  degree  of  stability.  To  guarantee  stability,  the 
usable  working  volume  for  each  leg  is  reduced.  Figure  2.5  shows  a  worst  case 
situation  demonstrating  that,  if  the  legs  are  confined  to  their  respective 
constrained  working  volumes,  the  vertical  projection  of  the  center  of  gravity  will 
always  fall  within  the  triangular  pattern  formed  by  the  supporting  legs.  A  further 
discussion  of  the  constrained  working  volume  can  be  found  in  [Ref.  4]. 

E.     GRAPHICS 

There  is  a  wide  spectrum  of  available  options  from  which  to  choose  in  the 
field  of  graphic  displays.  Decisions  are  required  as  to  running  the  simulation  on 
monochrome  or  color  monitors,  the  type  and  number  of  dimensions  for  the 
projection,  the  use  of  line  or  solid  figure  representation,  acceptable  display 
resolution  and  update  time,  and  whether  to  employ  special  hardware  options. 
State  of  the  art  graphics  machines  also  offer  possibilities  which  include  shading, 
reflectivity  of  surfaces,  and  multiple  light  sources.  A  compromise  must  be  made 
between  functionality,  visual  realism,  and  cost  in  order  to  realize  an  effective 
simulation. 

Past  simulation  models  featuring  the  ASV  [Ref.  4.21.22]  have  concentrated  on 

basic  functionality  in  the  display.    The  vehicles  and  terrain  were  represented  by 

simplified  line  drawings  on  a  monochrome  monitor.    This  study  attempts  to  take 

advantage  of  recent  developments  in  special  hardware  and  software  for  graphics 

workstations,  in  order  to  create  a  more  realistic  and  convincing  simulation.    It  is 
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Figure  2.5    Constrained  Working  Volumes 
for  Adaptive  Suspension  Vehicle 
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simulation.  It  is  believed  that  the  IRIS-2400  [Ref.  23]  represents  a  good 
compromise  between  state  of  the  art  quality,  cost,  processing  time  and 
availability.    This  system  was  therefore  selected  to  support  the  work  of  this  thesis. 

F.     SUMMARY 

This  chapter  provides  background  information  on  previous  research  leading  to 
this  study.  Discussions  include  a  brief  survey  of  examples  of  the  state-of-the-art 
walking  machines,  follow-the-leader  and  tripod  gaits,  vehicle  steering,  and  the 
question  of  stability  for  walking  machines.  In  addition,  several  concerns  are 
expressed  regarding  the  graphics  displays  used  to  portray  the  action  of  the 
walking  vehicles. 

The  following  chapter  contains  a  detailed  statement  of  the  ASV  simulation 
problem  to  be  solved  in  this  thesis. 
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III.    DETAILED  PROBLEM  STATEMENT 

A.  INTRODUCTION 

This  chapter  is  intended  as  a  description  of  the  nature  of  the  simulation 
modeling  problem.  In  it  is  a  discussion  of  the  configuration  of  the  ASV,  the 
mathematics  governing  its  motion,  and  foothold  selection  and  steering  algorithms 
for  two  selected  gaits.  Also  covered  are  the  simplifications  deemed  necessary  in 
the  creation  of  the  model.  The  final  section  includes  a  brief  description  of  the 
modeling  facilities. 

B.  ASV  CONFIGURATION 

The  Adaptive  Suspension  Vehicle  (ASV)  is  a  self-contained,  six-legged 
walking  machine  designed  to  traverse  uneven  terrain.  The  operator,  sitting  in  a 
cockpit  at  the  front  of  the  vehicle,  controls  the  vehicle  either  at  a  supervisory  level 
by  selecting  body  translational  and  rotational  velocities  and  allowing  the  vehicle 
to  automatically  place  the  feet,  or  by  coordinating  the  individual  legs  in  a 
precision-footing  mode.    The  various  control  modes  are  discussed  in  [Ref.  10]. 

The  vehicle  is  equipped  with  an  optical  scanning  rangefinder.  mounted  above 
the  cab.  for  short-range  sensing.  The  laser  rangefinder  has  a  range  of  10  m  and  a 
field  of  view  of  40  degrees  on  each  side  of  the  body  axis,  and  from  15  to  75  degrees 
below  the  horizontal.  [Ref.  10:  p. 8] 
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A  single  900  cc  four-cylinder  motorcycle  engine  is  sufficient  to  power  the  ASY 
over  sustained  periods  of  time.  This  is  possible  due  to  the  aluminum  construction 
of  the  frame  and  legs,  which  make  the  vehicle  relatively  light  (2700  kg)  for  its  size 
(3.0  m  height.  5.2  m  length)  [Ref.  10:pp.  8-10].  Power  is  distributed  to  eighteen 
hydraulic  actuator  pumps  through  an  energy  storage  flywheel  and  a  series  of 
shafts  and  toothed  belts. 

Seventeen  Intel  86/30  single-board  computers  are  used  for  onboard  processing 
and  control.  One  board  is  dedicated  to  each  leg  for  motion  control  and  leg  sensor 
data  processing.  Four  more  boards  compute  stability,  check  actuator  motion 
limits,  and  generate  leg  commands  based  on  the  operator's  control  inputs  and  the 
internal  terrain  model.  Two  additional  boards  are  used  for  cockpit  displays  and 
controls.  The  terrain  model  is  generated  by  the  remaining  five  single-board 
computers  using  the  data  gathered  from  the  optical  rangefinder.  [Ref.  10:  pp.  8- 
10] 

The  design  of  the  ASV's  legs  features  a  two-dimensioned  pantograph 
mounted  on  a  baseplate  hinged  to  the  body  (Fig.  3.1  and  3.2).  This  design  offers 
the  advantages  of  energy  efficiency  resulting  from  decoupled  ground  reaction  force 
components,  and  simplicity  of  control  [Ref.  5.6.  and  24].  Vertical  and  horizontal 
motion  relative  to  the  baseplate  are  provided  by  independent  actuators  mounted 
to  the  plate.  Abduction  and  adduction  motion  is  provided  by  a  third  actuator 
mounted  on  the  body. 
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Figure  3.1    ASV  Leg  Configuration  (1  of  2) 
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Figure  3.2    ASV  Leg  Configuration  (2  of  2) 
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C.    SELECTED  WALKING  ALGORITHMS 
1.      Gaits 

The  model  used  in  this  simulation  study  currently  supports  two  styles  of 
walking  patterns.  The  first,  closely  following  that  used  by  Lee  [Ref.  4]  features  a 
periodic  tripod  forward  wave  gait.  The  second  is  a  periodic  tripod 
follow -the  leader  (FTL)  gait  [Ref.  3].  Both  gaits  have  unique  advantages  to 
offer  the  operator. 

The  great  advantage  inherent  in  the  forward  wave  gait  lies  in  the 
maneuverability  it  offers  the  walking  vehicle.  The  ASV.  operating  in  the  forward 
wave  gait  mode,  is  free  to  place  its  feet  anywhere  within  a  constrained  working 
volume  during  the  leg  placement  phase  of  the  walking  cycle  [Ref.  4:pp. 59-62]. 
This  freedom  allows  the  vehicle  great  flexibility  in  range  of  movement;  even  to  the 
point  of  permitting  turning  in  place. 

The  price  for  this  freedom  of  choice  for  leg  placement  is  that  a  foothold 
must  be  found  and  tested  for  each  time  a  leg  is  placed  on  the  ground.  In  rough  or 
obscured  terrain  the  process  of  probing  and  testing  could  occupy  virtually  all  of 
the  vehicle's  onboard  processing  capability.  Thus,  the  vehicle's  speed  over  ground 
could  be  severely  limited. 

In    this    type    of    terrain    the    follow-the-leader    gait    could    prove    more 

advantageous.     The  follow-the-leader  gait   requires  probing  and  testing  only  for 

the  forward  two  legs.    Since  the  following   legs  step  precisely  where  the  leading 

legs  have  gone,  no  further  searching  is  needed.    On  difficult  or  dangerous  terrain. 
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where  extensive  probing  and  testing  of  foothold  is  required,  the  FTL  gait  promises 
both  greater  ground  speeds  and  more  security. 

The   notable    disadvantage   of  the    FTL   gait    is    that    its   use   drastically 
constrains   the   vehicle's   movement.     Maneuvers  such  as  sideways  stepping  and 
turning  in   place   are  not   possible.     Turning  the  vehicle   requires   a   large  radius 
turning  circle,  similar  to  that  needed  by  a  tractor  pulling  a  long  trailer. 
2.      Steering 

The  two  walking  algorithms  utilize  different  control  schemes  matching  the 
\mique  gait  characteristics.  The  forward  wave  gait  steering  mode  allows  the 
operator  to  independently  specify  longitudinal  velocity,  lateral  velocity,  and 
azimuth  angle  rate  (ideally  using  a  three-axis  joystick).  This  allows  the  operator 
to  take  fully  advantage  of  the  gait's  maneuverability.  In  the  absence  of  a  three- 
axis  joystick  for  this  simulation,  these  body  translation  and  rotation  rates  are 
input  through  three  sliding  bar  controls  using  a  mouse-driven  cursor  on  the 
display  screen. 

The    vehicle    in    the    follow-the-leader    gait     mode,    with     its     inherent 

restriction  that  the  body  remain  between  the  two  parallel  foothold  tracks,  behaves 

very  much  like  a  tractor  and  trailer  or  a  wagon.    Just  as  the  truck  driver  steers 

the  cab  allowing  the  trailer  to  follow  in  its  path,  the  ASV  operator  in  this  mode 

steers  by  specifying  the  desired  motion  of  the  vehicle  steering  point.    This  point 

lies  just  behind  the  cockpit,  mid-way  between  the  two  front  legs,  along  the  line 

joining  the  centers  of  the  two  working  volumes.    In  the  place  of  a  steering  wheel 
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and  acceleration  pedal,  the  operator  uses  a  two-axis  joystick  (simulated  with  a 
mouse-driven  cursor  on  a  steering  pad),  to  specify  the  desired  magnitude  and 
direction  of  the  cockpit's  relative  velocity  vector. 

The  truck  and  trailer  or  wagon  style  steering  commands  are  translated 
into  desired  longitudinal  and  lateral  translation  and  azimuth  rotation  rates  in 
order  to  maintain  compatibility  with  the  wave  gait  control  algorithm  in  the 
program.  This  is  done  by  first  transforming  the  steering  point  (vehicle  head) 
actual  position  and  desired  velocity  to  Earth  coordinates,  {*hr-  yhE~  ~hE)  and 
(xdhE-  y<ihE-  'zdhE)  respectively.  The  desired  cockpit  position  (x^,  ydh£.  zdhE)  is 
determined  by 

XdhE  =~~   XhE   +  XdhE  '  A/  (3-1) 

vaE =  yhE +  ydhE  •  A/  (3-2) 

ZdhE   "    ZhE    "   ZdhE  '  A/  (3-3) 

where  At  is  the  program  display  time  increment.  Using  the  desired  cockpit 
position  and  the  centroid  of  the  middle  and  rear  legs'  footholds  (in  Earth 
coordinates)  (fh  ,fh  Jh),  the  desired  azimuth  angle  $ d  is  obtained. 


^  d  =  tan 


VdhE  ~  fhi 


xdhE  '  fhz . 


(3.4) 
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The  desired  new  position  of  the  body's  center  {^dE-ydE-  *</£)'  *s  tnen  f°und  by 


L 

xdE 

XdhE 

— cos\^ 
2 

L 

ydE " 

^dhE   " 

2 

ZdE 

ZE 

(3.6) 

(3.7) 

where  L  is  the  length  between  the  center  of  the  working  volumes  of  the  forward 
and  rear  legs. 

The   desired   Earth   translation   rates   (xd£   and    ydE]    and   Euler   azimuth 
angle  rate  (w  .  E)  are  determined  as 

XE 

(3.8) 


XdE   ~~  XE 

XdE  ' 

At 

ydE  -  Ve 

VdE    - 

At 

*d   * 

•""'dzE 

A* 

(3.10) 

with  ^  being  the  current  azimuth  angle.    These  Earth  and  Euler  rates  are  then 
translated  to  body  rates  (i,„,  y  ,„,  ^j,d)  by 

xdB  =  xdEcos*  T  ^£sin*  (3-U) 

ydB  =  ydEzo^-  xdE^  (312) 
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3.     Foothold  Selection 

As  indicated  in  the  above  section,  a  new  foothold  must  be  selected  for 
each  leg  while  operating  in  the  forward  wave  gait  mode.  In  order  to  maximize  the 
foothold"s  usefulness  it  should  be  placed  so  that  the  foot  remains  in  the 
constrained  working  volume  during  the  leg's  support  phase  for  a  maximum  length 
of  time.  The  optimal  foothold  position  is  determined  as  the  "point  on  the  surface 
of  the  constrained  working  volume  such  that  [the  leg's]  support  trajectory  is 
predicted  to  pass  through  the  foot  reference  position"  [Ref.  4:  p.  100].  To  simplify 
the  computation,  the  reference  position  is  taken  as  the  center  of  the  working 
volume  and  a  straight  line  is  used  to  approximate  the  foot  trajectory.  A  line  is 
projected  opposite  to  the  direction  of  the  predicted  foot  velocity  vector  at  the 
reference  point.  The  intersection  of  this  line  and  the  boundaries  of  the 
constrained  working  volume  is  then  the  desired  foot  position.  Subsequent 
variation  of  the  body  velocity  will  alter  the  supporting  foot  trajectory,  potentially 
resulting  in  a  suboptimal  foothold. 

The  follow-the-leader  gait  foothold  selection  process  is  much  different. 
New  footholds  for  the  leading  two  legs  are  found  by  projecting  a  line  along  the 
velocity  vector  of  the  vehicle's  cockpit.  At  a  set  distance  (1/12  the  length 
between  the  forward  and  rear  hip  joints),  along  this  line,  another  line 
perpendicular  to  it  is  projected.  This  distance  is  one  half  the  leg  stroke  of  the 
vehicle  while  operating  with  a  pitch  of  1/3  (Figure  2.2). 
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The  desired  foothold  is  determined  by  where  the  second  line  intersects  a  line 
running  through  the  center  of  the  working  volume  parallel  to  the  body's 
longitudinal  axis  (Figure  3.3). 

As  a  front  leg  abandons  its  current  foothold,  that  position  is  recorded  for 
use  by  the  middle  leg  behind  it.  In  turn,  the  middle  leg  foothold  positions  are 
saved  for  use  by  the  rear  legs.  Thus,  during  each  complete  leg  cycle,  two  new 
foothold  positions  are  computed.  This  compares  favorably  to  the  six  new 
footholds  needed  while  using;  the  forward  wave  gait. 


selected 
foothold 


longitudinal 
axis  of  the 
working  volume 


relative  heading 


B 


Figure  3.3   New  Foothold  Location 
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The  current  program  allows  the  driver  to  start  operation  of  the  vehicle 
either  using  the  forward  wave  gait  or  the  follow-the-leader  gait.  Once  started,  the 
program  must  be  reset  before  switching  modes. 

D.    MODELING  SIMPLIFICATIONS 

There  are  many  simplifying  assumptions  contained  within  this  model  of  the 
ASV.  These  simplifications  were  made  largely  in  an  effort  to  speed  the 
development  of  such  features  as  the  follow-the-leader  gait.  However,  the  program 
framework  was  devised  with  future  work  in  mind.  Thus,  wherever  possible  room 
was  left  for  generalization  and  expansion. 

The  most  notable  simplification  in  the  simulation  model  deals  with  terrain. 
The  ground  is  represented  by  a  smooth,  level,  checkerboard  pattern.  Although 
the  ASV  was  developed  to  be  able  to  traverse  unstructured  terrain,  there  are  no 
obstacles  or  obstructions  in  the  current  model.  Uneven  terrain  will  require 
inclusion  of  an  algorithm  for  estimating  the  support  plane  beneath  the  vehicle, 
foot  sensors,  and  a  new  terrain  display  routine.  A  foothold  probe  and  testing 
routine  will  also  be  needed. 

As  a  consequence  of  the  use  of  fiat  terrain,  the  constrained  working  volume 
adjustments  for  uneven  terrain  [Ref.  4:  pp.  109-117]  and  body  regulation  plans  for 
varying  slopes  [Ref.  4:  pp.  87-  )\  were  not  required.  However,  the  basic  structure 
for  body  attitude    and    altitude  regulation  has  been  retained  in  the  program 
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modules     of    this     thesis.      Consequently,     inclusion     of    sloped     terrain     should 
necessitate  only  minor  program  changes  in  these  areas. 

The  model  contains  only  kinematic  features  of  the  ASV  operation.  This 
means  that  there  are  no  limits  on  vehicle  acceleration  imposed  by  the  model.  In 
order  to  prevent  unrealistic  performance  on  the  part  of  the  displayed  vehicle,  a 
filter  was  placed  between  the  commanded  inputs  and  the  response  of  the  vehicle. 
The  kinematics  and  filter  for  simulating  dynamic  constraints  are  described  in  the 
section  below. 


E.    MODEL  KINEMATICS 

The  kinematics  of  the  model  of  the  ASV  presented  here  closely  follow  those 
developed  in  the  computer  simulation  of  Lee  [Ref.  4].  Body  motion  is  specified  in 
terms  of  translation  rates  along  the  body's  forward,  lateral,  and  vertical  axes  (x. 
y.  and  z  repectively)  and  rotation  rates  around  these  axes.  The  driver  of  the 
vehicle  may  directly  or  indirectly  con  )1  the  desired  forward  and  lateral 
translation  rates  and  the  rotation  rate  around  the  vertical  axis.  The  remaining 
three  degrees  of  freedom  are  automatically  regulated  to  maintain  a  desired  body 
attitude  and  altitude  with  respect  to  the  ground. 

Vehicle"  "dynamics  are  simulated  through  the  use  of  a  simple  control  filter 
inserted  between  the  ordered  rates  and  the  actual  bodv  rates.     As  a  result  the 
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body  moves  with  a  smooth,  exponential  transition  in  response  to  driver  and  body 
regulator  control  commands. 

In  order  to  realize  these  filtered  body  rates,  the  rates  are  first  converted  to 
earth  coordinate  translation  rates  and  body  Euler  angle  rates.    Euler  integration  is 
then  performed  to  produce  translation  distances  in  earth  coordinates  and  angular 
displacement  around  the  three  body  Euler  axes. 
1.      Coordinate  Systems 

The  ASV  model  makes  use  of  two  coordinate  systems,  earth  (xE.  yF,  zF) 
and  body  (x„.  yB.  :{i).  in  its  calculations.  The  earth  coordinate  system  is  used 
wherever  it  is  required  to  specify  absolute  position  or  velocities  of  the  body.  feet, 
or  terrain.  The  earth  coordinate  system  is  defined  such  that  the  zE  axis  is  positive 
upward  and  the  unit  vectors  xF,  yE  and  zE  are  mutually  orthogonal. 

The  body  coordinate  system  is  useful  in  describing  operator  control  and 
the  coordination  of  body  and  legs.  The  origin  is  defined  as  the  center  of  the  main 
body  section  (excluding  the  cockpit).  The  z„  axis  is  projected  upward  through 
the  top  of  the  body,  while  the  ;„  axis  is  forward  along  the  longitudinal  axis  and 
the  yR    axis  is  projected  to  the  body's  left,  forming  the  transverse  or  lateral  axis. 

Earth  coordinates  are  transformed  to  body  coordinates  using  the 
relationship  of  equation  3.14. 
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XB 

yE 

=  H 

Vb 

ZE 

ZB 

1 

1 

(3.14) 


T  T 

where  the  position  vectors  [xE,  yE,  z£.  l]      and  [xg,  yg.  zg,  l]       describe  the  same 

point    in   space   in   earth   and   body   coordinates   respectively   and    H    is   a   4   x   4 

homogeneous  transformation  matrix  [Ref.  25].    The  homogeneous  transformation 

matrix  can  be  derived  by  decomposing  the  transformation  into  a  translation  from 

the  earth  coordinate  origin  and  a  series  of  rotations  about  the  Euler  axes: 


xyz        W        <P        fc) 


(3.11 


The  homogeneous  transformation  matrix  T  y  represents  the  translation 
of  the  body's  center  to  its  current  position  (d  ,  d  .  d_).  The  first  rotation  about 
the  body's  vertical  axis  by  the  azimuth  angle  $  is  represented  by  the  matrix  T^. 
The  body  is  then  rotated  about  its  new  lateral  axis  by  the  elevation  angle,  $.  and 
then  about  the  newly  formed  longitudinal  axis  by  the  roll  angle.  0.1 


1  Other  notations  are  sometimes  used  for  these  angles.    For  example,  in  [Ref.  19],  8  signifies 
elevation  angle  and  Q  denotes  roll  angle. 
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The  four  homogeneous  transformation  matrices  [Ref.  25:  p30J  are: 
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Substituting  equations  3.16,  3.17.  3.18.  and  3.19  into  3.15  yields: 


H 


cos^cos$    cos^sin^sinO-sin^cos©    sin^sinQ-rcosN&sin^cos©    d 
sinVfcos^    cos^cosQ  +  sin^sin^sin©    sin\I/sin<I>cos@    cos\I>sin©    d 


sin<& 


cos<J>sin@ 


cos$cos© 


;3.20) 


2.     Body  Regulation 

A  simple  control  algorithm  is  used  in  this  and  Lee's  model  to  maintain 
the  attitude  of  the  vehicle  and  its  height  above  the  ground.  The  inputs  are  the 
estimated  support  plane  and  the  plane  formed  by  the  body's  lateral  and 
longitudinal  axes. 

Body   attitude   regulation  is  accomplished  by  rotating  the  present  body 
plane  towards  the  desired  body  plane.    The  desired  plane  can  be  expressed  as  a 
function  of  the  terrain  slope  and  be  adjusted  to  suit  the  driver0. 
The  unit  vector  B,  along  the  rotation  axis.  (Fig.  3.4)  is  given  by 


Bk- 


'B     ~D 


zbxzd\ 


[*,  K  K\ 


[3.21) 


where   zB   and   zD   are  the  unit  normal  vectors  of  the  current  and  desired  body 


"  In  the  current  level  terrain  model,  the  desired  body  plane  angle  is  set  equal  to  zero. 
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Figure  3.4   Rotation  Axis  and  Angle 


planes  and  k    =  0.  The  rotation  angle,  7,  is  given  by 


7  =  cos_1(i5  •  zD) 


(3.22) 


These  values  are  used  in  the  control  function  to  obtain  the  rotation  rates  around 
the  body's  longitudinal  axis,  jj  .  and  about  its  transverse  axis  u  . 

Body    altitude    is    defined    as    the    distance   along    the   body    plane's   unit 
norma,     ora  the  estimated  support  plane  to  the  body's  center  of  gravity. 
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A  mapping  function  similar  to  that  used  for  the  body  plane  can  be  used  to  relate 
the  desired  altitude.  hD.  to  the  current  terrain  slope,  h4 
3.      Rate  Computation 

The  differential  equation  describing  the  simulated  dynamics  of  the  control 
filter  is 

1 

y{t)  --      ~  y{t)  (3.23) 

r 

where  r  is  the  time  constant  of  motion  and  y(t)  is  difference  between  the  desired 
and  actual  position  variable.  Integrating  both  sides  of  equation  3.10  yields  an 
exponential  response 


l 


y[t)  -  '   r 


The  control  filter  for  altitude  is  then 


(3.24) 


1 
ZB  ~    ~~  (hD~h)-  (3.25) 

ri 

Similarly  the  equation  producing  the  attitude  control  response  is 

1 

7  =  -—  7-  (3.26) 

T 


In  the  current  model  the  desired  height  is  set  to  a  constant  value 
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The  rotational  vector  7,   decomposes  into  rotation  vectors  about  the  forward  and 
lateral  axes,  yielding: 

1 


j 

T 


~  kS  (3.27) 


1 
w„  =  ~~  kv1'  (3.28) 

Velocity     is    related    to    acceleration    using    the    same    filter.      This    is 
accomplished  by  letting 

y{t)  =  i(t)  (3.29) 

and  substituting  into  equation  3.23,  yielding: 


£  1 

C  x{i)  --    -—  x{t).  (3.30) 
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The  accelerations  for  the  remaining  three  rates  are 
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lTsing  the  frrrear  approximation. 

Avelocity  ~  Aff'me- acceleration,  (3.34) 

the  rates  are  determined  by  equations  3.35  through  3.37. 
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(3. 36] 


[3.37) 


where  At  is  the  time  increment  and  r„  is  the  time  constant  of  motion. 

Body  positioning  in  this  computer  model  is  achieved  by  translating  the 
body  center  to  its  proper  earth  coordinate  position  and  then  successively  rotating 
the  body  about  its  vertical,  transverse  and  longitudinal  axes.  In  order  to  do  this, 
body  rates  are  first  transformed  into  earth  coordinate  translation  rates  and  body 
Euler  angle  rates  using  the  method  presented  by  Frank  and  McGhee  [Ref.  19]. 


1    tan^sin©    tan$cos© 
0        cos©  sin© 

0    sec$sin©    sec^cos© 


(3.38) 


Roll,    elevation    and    azimuth    angles    and    translation    distances    are   then    found 
through  simple  Euler  integration: 


VniH  "+  y  '  Atime. 


(3.39) 
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4.      Leg  Kinematics 

The  ASV's  pantograph  leg  contraction  yields  relatively  simple  kinematic 
and  inverse  kinematic  equations.  These  equations  differ  slightly  from  those 
presented  by  Lee  >Ref.  4].  This  can  be  attributed  to  the  use  of  more  accurate 
dimension  measurement  than  those  assumed  by  Lee. 

For  the  front  left  leg  (leg  number  one)  shown  in  (Fig.  3.1  and  3.2).  the 
foot  position  is  given  by 

i,  =  od2  +  \  (3.40) 

y,  ----  (5/3  -  4<f,)sin0  -f  /4cos0  -  h}j  (3.41) 

z.  ----  /4sin0  +  (5/3    -  4d,)cos0  (3.42) 

where  the  hip  position  [h  .  h  .  h)  and  foot  position  (x,.  y,.  z,)  are  given  in    body 
coordinates,  and  d     d„,  and  0  are  the  joint  variables. 

The  inverse  kinematic  equations  for  the  joint  variable  gL,  derived  from 
equation  3.40  is 

1 
d.2  -    ~  (xf  -  ht).  (3.43) 

5 

Rearranging  and  squaring  both  sides  of  equations  3.41  and  3.42  yields, 

(/'sin"0  -  a  /4sin0cos0  +  /^cos'0  =  (yf  -  h  )'  (3-44) 

/4sin"0~-  a  /4sin0cos0  -  g"cos"0  =  (2.  -  hy)  (3.45) 
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whore  a       (5L  -  Ad  .).    Solving  equations  3.31  and  3.32  gives. 
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a'  +  /, 


[3.46) 


(3.47) 


In  addition  to  the  joint  parameters,  this  model  requires  leg  upper  (thigh) 
angle,  a.  the  lower  leg  (shank)  angle,  7,  and  the  knee  position  in  body  coordinates 
(x,.  y,.  z,).    The  thigh  angle  is  given  in  terms  of  joint  variables  as 


7T 

a  -    —    -  tan 


dn      \ 


/3  +  rfJ 


cos 


', 

- 

V    <    d2 

-L 

K 

*  y ) 

2 

'1 

v7^  * 

d, 

0 

)  - 

*- 

(3.48) 


and  the  knee  position  as 


1,   =  Lcosq    -  // 

k  1  1 

yk  ■---  (/.-,sina       rfjsin©  -  /4cos0  +  h 
z,   -  /sin©  -  (/^sina  -  d  )cos0 

The  knee  an^le  is 


(3.49) 
(3.50) 
(3.51) 


-)  =  tan 
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{   x 


xf  ' 


0.0. 


All  six  legs  of  the  ASV  share  similar  geometries.  The  remaining 
kinematic  and  inverse  kinematic  equations  can  be  obtained  from  equations  3.40 
through  3.52  with  appropriate  sign  changes. 
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F.     SIMULATION  FACILITIES 
1.      Hardware 

The  computer  simulation  presented  here  is  designed  to  run  on  either  of 
the  two  Silicon  Graphics.  Inc.  IRIS-2400  workstations  currently  in  the  computer 
graphics  laboratory  in  the  Department  of  Computer  Science  at  the  Naval 
Postgraduate  School.  The  IRIS  (Integrated  Raster  Imaging  System)  consists  of  a 
Geometry  Pipeline,  a  general  purpose  microprocessor,  a  raster  subsystem,  a  60Hz 
non-interlaced  high-resolution  RGB  display  monitor  and  a  keyboard.  In  addition 
each  unit  has  been  equipped  with  two  72  megabyte  disk  drives,  a  cartridge  tape 
unit,  a  Moating  point  accelerator,  and  a  three-input  mouse.  The  Geometry 
Pipeline  is  a  series  of  ten  or  twelve  custom  VLSI  chip  matrix  multipliers.  L'nder 
the  control  of  the  applications  graphics  processor,  it  performs  matrix 
transformations,  clipping  and  scaling  of  coordinates.  The  output  is  sent  to  the 
raster  subsystem  which  performs  functions  such  as  filling  in  pixels,  shading, 
depth-cueing  and  hidden  surface  removal. 

The  first  IRIS  system  is  based  on  a  Motorola  MC68010  processor  with  5 
megabytes  of  CPU  memory  and  a  1024  x  786  x  8  bit  display  memory.  It  is  also 
equipped  with  a  digitizer  tablet.  The  second  IRIS  system  is  a  more  capable 
Turbo-2400.  It  is  based  on  a  Motorola  MC68020  processor  and  has  4  megabytes 
of  CPU  memory  and  a  1024  x  768  x  32  bit  display  memory.  An  Ethernet  network 
connects  both  workstations  to  two  VAX  11/780's  and  one  VAX  11/750. 
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2.      Software 

The  IRIS  Graphics  Library  contains  a  large  number  of  graphics 
commands  and  utilities.  This  allows  the  user  great  flexibility  in  the  choice  of 
coordinate  systems  and  display  techniques.  While  the  software  is  written  in  C. 
the  graphics  commands  may  be  called  in  C.  FORTRAN,  Pascal,  and  Lisp.  The 
code  for  the  model  presented  in  this  study  is  written  exclusively  in  C. 

G.    SUMMARY 

The  previous  sections  of  this  chapter  outlined  the  physical  constraints, 
simplifications,  and  tools  used  in  the  development  of  this  simulation.  The  next 
chapter  describes  the  operation  and  construction  of  the  actual  simulation 
program. 
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IV.    SIMULATION  PROGRAM 

A.  INTRODUCTION 

In  this  chapter  the  simulation  program  is  presented.  The  first  section  consists 
of  a  user's  guide,  with  complete  instructions  on  how  to  use  each  program  feature. 
The  second  section  introduces  the  working  environment  for  graphics  on  the  IRIS- 
2400.  The  final  section  describes  the  internal  operation  of  the  simulation  program 
and  discusses  the  flow  through  the  major  modules.  A  complete  listing  of  the 
program  is  provided  in  the  appendix. 

B.  USER'S  GUIDE 
1.      Starting  Up 

The  program  walk.c  is  relatively  simple  to  use.  It  is  entirely  menu-driven, 
with  a  single  mouse  button  and  cursor  performing  all  selection  functions.  To  start 
the  program,  type  the  command  "walk". 

Immediately  displayed  on  the  monitor  is  a  split  screen  view  of  the  control 

panel  and  the  ASV  on  its  terrain  (Fig.  4.1).    The  right  half  of  the  screen  features 

a   three-dimensional  projection  of  the  ASV  on  a  green  and  white  checkerboard 

plane  against  a  blue  backdrop.    The  user's  vantage  point  is  fixed  relative  to  the 

center  of  gravity  of  the  vehicle  (above  and  initially  to  the  vehicle's  left  side),  so 

that  the  vehicle  will  continuously  remain  in  view  while  walking. 
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The  left  half  of  The  screen  contains  a  two-dimensional  representation  of 
the  control  panel.  Initially  it  features  only  the  six  yellow  selection  panels  of  the 
main  menu  on  a  cyan  background. 

2.  Menus 

A  menu  item  is  selected  by  placing  the  cursor  over  the  corresponding 
panel  and  clicking  the  middle  mouse  button.  Pressing  the  button  down  will  cause 
the  panel  beneath  the  cursor  to  be  highlighted  in  red.  as  a  potential  choice. 
Releasing  the  button  selects  the  highlighted  menu  item.  If  no  changes  are  desired 
in  the  current  menu  selection,  simply  move  the  cursor  to  a  portion  of  the  screen 
outside  the  menu  selection  region  and  release  the  mouse  button.  Selected  menu 
items  are  highlighted  in  bright  yellow. 

3.  Forward  Wave  Gait 

In  the  forward  wave  gait  mode,  vehicle  velocities  are  specified  in  terms  of 
body  axis  translation  and  rotation  rates.  Three  of  these  rates  -  longitudinal 
velocity,  lateral  velocity,  and  yaw  rate,  are  directly  controllable  by  the  operator. 
The  rates  for  the  remaining  three  degrees  of  freedom  are  automatically  adjusted 
by  the  vehicle  in  order  to  maintain  proper  attitude  and  altitude.  All  rates  in  this 
mode  are  defined  with  respect  to  the  body's  center  of  gravity. 

Selecting    the    forward    wave    gait    panel    produces    a    secondary    menu 

displayed    immediately   below   the   main   menu    (Fig.   4.2).   This  secondary  menu 

contains  three  additional  panels  for  use  in  specifying  the  vehicle's  body  rates.    The 

panels  are  operated  in  the  same  manner  as  those  in  the  main  menu.    To  the  right 
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of   the   menu    panels   are   six   simulated   LED    readouts,    used   for   displaying   the 
magnitude  of  the  current  and  ordered  rates. 

Releasing  the  middle  mouse  button  while  the  cursor  is  inside  the  bounds 
of  one  of  the  secondary  menu  panels  results  in  a  sliding  bar  control  panel  being 
displayed  on  the  left  edge  of  the  screen  (Fig.  4.3).  Velocity  commands  are  input 
by  placing  the  cursor  within  the  black  center  region  of  the  bar  control  area.  A 
yellow  bar  level  indicator  will  rise  or  fall  to  match  the  cursor  level,  indicating  the 
commanded  velocity  value.  No  clicking  of  the  mouse  button  is  required.  To  set 
the  commanded  input  at  the  desired  level,  move  the  cursor  to  the  desired  height 
and  then  slide  the  cursor  horizontally  until  it  is  outside  the  center  region  of  the 
sliding  bar  panel.  A  red  bar  level  indicator  displays  the  current  velocity  of  the 
vehicle. 

4.      Follow-the-Leader  Gait 

Control  while  in  the  follow-the-leader  gait  mode  is  achieved  by  specifying 
the  desired  relative  velocity  vector  of  the  ASV's  steering  point.  The  operator,  in 
essence,  points  the  vector  in  the  direction  in  which  the  steering  point  should 
travel,  relative  to  the  body  longitudinal  axis.  As  stated  in  the  previous  chapter, 
the  steering  is  very  much  like  that  of  a  long  tractor-trailer  type  of  vehicle.  The 
control  algorithm  factors  in  the  magnitude  of  the  desired  velocity,  footholds  and 
current  velocity  and  automatically  regulates  the  body's  motion. 
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The  follow-the-leader  gait  control  mode  is  invoked  by  using  the  lower  left 
panel  of  the  main  menu.  When  this  panel  is  selected,  a  white  rectangular  control 
area  appears  directly  beneath  the  main  menu  (Fig.  4.4).  If  the  middle  mouse 
button  is  held  down  while  the  cursor  is  within  this  region,  the  cursor  controls  a 
simulated  two-axis  joystick.  The  vertical  axis  represents  the  magnitude  of  the 
relative  velocity  vector  and  the  horizontal  axis  represents  the  direction.  A  solid 
yellow  line  is  used  to  indicate  the  current  joystick  position,  and  thus  the  input 
values.  The  vehicle's  actual  relative  cockpit  velocity  vector  is  indicated  by  a  solid 
red  line. 

5.      Status  and  Warnings 

The  status  menu  option  exists  to  provide  the  operator  with  numerical 
data  on  leg  and  body  position  and  movements.  Selecting  this  item  causes  a 
yellow  and  black  display  panel  to  appear  below  the  main  menu  (Fig.  4.5). 
Featured  on  this  panel  are  the  translation  and  rotation  rates  (with  respect  to  the 
body  axes),  the  position  of  the  vehicle's  center  of  gravity  (in  Earth  coordinates), 
the  vehicle's  orientation  (in  Euler  angles),  the  walking  cycle  period,  the  position  of 
each  foot  (in  body  coordinates)  and  the  angles  of  various  components  of  the  legs. 
The  values  are  updated  each  display  cycle. 

During    the    operation    of   the    vehicle,    checks    are    made    on    operating 

parameters.      If    a     leg    become    positioned    so    that     the    foot     is    outside    its 

corresponding  constrained  working  volume,  a  red  warning  box  is  flashed  in  the 

lower  left  corner  of  the  screen.    Similarly,  if  the  walking  cycle  period  becomes  too 
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small,    a    yellow    warning    box    is    displayed.      In    addition    to    the    warning,    a 
deceleration  routine  is  activated  to  slow  the  vehicle  until  the  period  comes  up  to 
an  acceptable  level  [Ref.  4:  pp.  66-71]. 
6.      Reset  and  Exit 

The  reset  option  returns  all  vehicle  parameters,  including  position,  to 
their  original  values.  This  feature  was  included  to  save  time  when  making  a  series 
of  test  runs.  The  exit  option  ends  the  program,  clears  the  screen  and  returns  the 
user  to  the  current  UNIX  shell. 

C.    GRAPHICS  ON  THE  IRIS-2400 

Figures  are  displayed  on  the  IRIS-2400  by  calling  a  series  of  short  graphics 
commands,  called  primatives.  The  primatives  are  interpreted  into  graphical 
displays  by  the  software  and  special  hardware  of  the  IRIS  system.  These  include 
commands  for  specifying  color,  drawing  lines,  circles,  irregular  polygons,  and 
printing  text  characters  on  the  screen.  There  are  also  a  series  of  primatives 
designed  to  manipulate  coordinate  transformation  matrices  for  the  purpose  of 
scaling,  rotating  and  translating  figures. 

A  sequence  of  graphics  commands  may  be  grouped  into  a  listing  called  an 
object.  This  object  list  may  then  be  conveniently  executed  using  a  single  call. 
Once  created,  the  object  list  may  at  any  time  be  edited  as  desired  through  the  use 
of  object  tags.  The  object,  in  essence,  functions  as  a  reconfigurable  graphics 
subroutine. 
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Objects  and  figures  in  this  program  are  displayed  in  the  reverse  order  in 
which  they  are  called.  The  last  object  is  overlaid  in  front  (with  respect  to  the 
viewer's  vantage  point),  of  the  previously  called  objects.  An  important  exception 
to  this  is  the  treatment  of  the  back  face  of  polygons.  The  reverse  side  of  a 
polygon,  in  the  back  face  polygon  removal  mode,  is  considered  transparent  and  is 
automatically  removed  from  the  image  as  a  hidden  surface  by  the  IRIS  hardware. 
This  feature  enables  the  display  of  more  realistic  appearing  three-dimensional 
objects. 

In  the  double  buffer  display  mode  utilized  by  this  program,  the  special  display 
memory  is  divided  into  two  sets  of  bit  plane  buffers.  As  one  buffer  is  having 
display  data  written  into  it.  the  other  is  used  to  refresh  the  monitor.  Once  the 
writing  is  complete,  the  functions  of  the  buffers  are  swapped,  and  a  new  cycle  of 
writing  commences.  This  display  mode  provides  for  a  smooth  simultaneous 
update  of  the  entire  screen. 

D.    PROGRAM  ORGANIZATION 

The     simulation     program    can     be     divided     into     three     general    sections; 

initialization,  simulation  loop,  and  termination.    The  heart  of  the  program,  the 

simulation   loop,  cycles  through  an   input   phase,  which  serves  as  the  operator's 

control  interface  for  the  vehicle:  the  calculation  phase,  in  which  the  parameters 

for  the  position  and  orientation  of  the  AS\r,s  body  and  legs  are  determined;  and  a 

display    phase.      The    initialization    section    performs    tasks,    such    as    defining 
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coordinate  systems  and  creating  object  lists,  required  to  start  up  the  loop.  The 
termination  section  clears  the  screen  and  buffers  in  preparation  for  the  next  IRIS 
user.  A  flow  chart  featuring  the  program's  primary  modules  is  shown  in  Figure 
4.6. 

Since  the  order  in  which  objects  are  called  is  critical  in  this  display  mode, 
special  provisions  are  needed  to  create  a  full  360  degree  viewing  coverage  of  the 
maneuvering  vehicle.  Specifically,  four  separate  ASV  objects  lists  are  created  in 
the  object  construction  module  of  the  initialization  section.  Each  object  has  the 
vehicle  components  ordered  for  proper  viewing  from  one  of  four  viewing 
quadrants.  The  display  section  therefore  needs  only  to  determine  from  which 
quadrant  the  vehicle  is  to  be  viewed  and  call  the  appropriate  object. 

The  simulation  loop  is  the  dominant  part  of  the  code,  containing  the 
overhelming  majority  of  the  program  modules.  The  loop  begins  with  a  call  to  the 
driver's  command  interface  module.  This  module  controls  the  operation  and 
display  of  the  menu  system,  the  status  panel,  and  the  sliding  bar  and  joystick 
controls,  as  well  as  processing  F.T.L.  gait  steering  commands.  The  organization 
of  the  control  module  is  shown  in  Figures  4.7  and  4.8. 
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Figure  4.6   Main  Simulation  Flowchart 
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Figure  4.7    Command  Interface  Module  (1  of  2) 
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Immediately  following  the  command  module  are  two  checks  on  the  program 
status.  If  the  exit  option  was  selected  in  the  command  module,  the  loop  is 
interrupted  and  the  program  enters  the  termination  stage.  The  reset  option 
causes  a  module  call  to  re-initialize  all  working  parameters. 

The  calculation  phase  of  the  loop  is  begins  with  the  support  module,  where  a 
determination  of  the  estimated  support  plane  directly  beneath  the  vehicle  is 
made.  The  position  and  velocity  of  the  ASV's  body  is  then  calculated  in  the 
body  rates  module  (Figs.  4.9  and  4.10).  The  body  kinematics  used  in  this  module 
are  discussed  in  section  III.E. 

The  gait  period  is  next  calculated  in  the  optimal  period  module.  This  module 
us.  -  a  optimal  period  control  algorithm  which  considers  the  kinematic  limit  of  the 
supporting  legs.  In  this  algorithm,  a  period  is  calculated  for  each  leg.  based  on 
the  time  required  for  its  foot  to  reach  the  limits  of  its  corresponding  constrained 
working  volume.  The  minimum  of  all  of  the  supporting  leg's  periods  is  chosen  as 
the  vehicle's  optimal  period.  No  foot  should  therefore  be  required  leave  its 
constrained  working  volume  while  supporting  the  body.  A  backward  gait  period 
is  also  computed  for  the  use  of  the  wave  gait  walking  in  the  reverse  direction. 
[Ref.  4:  pp.  63-69] 
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Figure  4.9    Body  Rates  Module  (1  of  2) 
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Figure  4.10   Body  Rates  Module  (2  of  2) 


69 


The  deceleration  module  checks  the  output  of  the  previous  module.  If  the 
period  is  below  the  set  threshold,  the  vehicle  is  slowed.  Each  time  the  period  falls 
below  the  minimum  value,  longitudinal  body  velocity  is  cut  ten  percent  and 
lateral  velocity  and  yaw  rate  are  cut  twenty  percent.  This  slowing  occurs  in  each 
pass  until  the  walking  period  rises  to  acceptable  limits. 

The  leg  phase  module  is  used  to  update  the  movement  phase  of  each  leg. 
The  phase,  expressed  as  a  modulo  one  floating  point  number,  indicates  at  what 
point  the  leg  is  in  its  cycle  of  supporting,  lifting  off  from  the  ground,  being 
transferred  toward  the  desired  foothold,  and  being  placed  onto  the  ground.  The 
relative  phases  of  the  legs  in  this  simulation  are  set  to  move  the  legs  in  two.  180 
degrees  out  of  phase  tripods. 

The  foot  trajectory  module  uses  the  leg  phase  information  and  the  period  in 
calculating  the  position  of  the  feet  relative  to  the  body.  The  algorithm  is  shown 
in  the  flow  chart  of  Figure  4.11.  The  transfer  time  is  the  length  of  time  allotted 
for  moving  the  foot  from  one  foothold  to  the  next.  This  determines  the  speed  in 
which  the  transfer  is  made. 

The  phase  of  the  leg  relative  to  the  beginning  of  foot  liftoff  is  referred  to  as 

the  transfer  phase.     When  the  leg's  transfer  phase  is  negative,  corresponding  to 

being  on  the  ground  in  a  supporting  role,  the  foot's  relative  position  is  determined 

by  the  support  trajectory  module   (Fig.  4.12).     When  the  leg's  transfer  phase  is 

greater  than  zero  but   less  than  the  liftoff-transfer  transition  value,  the  relative 

foot  position  is  returned  by  the  liftoff  trajectory  module  (Fig.  4.13).  Likewise  a 
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Figure  4.11   Foot  Trajectory  Module 
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Figure  4.12  Support  Trajectory  Module 
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Figure  4.13   Lift  Trajectory  Module 
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transfer  phase  value  between  the  two  transition  point  values  yields  a  response 
from  the  transfer  trajectory  module  (Fig.  4.14  and  4.15).  and  a  phase  value 
greater  than  the  transfer-placement  transition  value  yields  a  relative  foot  position 
calculation  from  the  placement  trajectory  module  (Fig.  4.16). 

The  foothold  selection  algorithms  contained  in  the  transfer  trajectory  module 
are  discussed  in  section  III.C.3.  Note  that  within  this  module  the  desired  end  foot 
position  is  treated  differently  in  the  follow-the-leader  and  forward  wave  gait 
modes.  The  forward  wave  gait,  with  its  high  degree  of  maneuverability,  has  a 
considerable  greater  probability  that  the  projected  ideal  position  toward  the  end 
of  the  transfer  phase  will  be  much  different  from  that  at  the  start.  Therefore  the 
desired  foot  position  in  the  case  of  the  forward  wave  gait  is  updated  on  each  pass. 
In  the  follow-the-leader  gait  case  it  is  only  calculated  during  the  first  time  through 
the  the  module. 

The  results  of  the  calculation  phase  are  the  position  and  orientation  of  the 
body  and  the  relative  position  of  each  of  the  feet.  These  values  are  used,  with  the 
inverse  kinematic  relations  derived  in  section  III.E.4.  in  the  display  phase  to 
obtain  the  rotation  angles  and  translation  distances  required  to  position  the 
ASV's  component  parts. 
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Figure  4.14  Transfer  Trajectory  Module  (1  or  2) 
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Figure  4.15  Transfer  Trajectory  Module  (2  of  2) 
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Figure  4.16  Placement  Trajectory  Module 
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The  ASV  object  lists  are  then  edited  and  the  updated  parameters  inserted 
into  their  corresponding  rotation  and  translation  commands.  Once  this  is 
completed,  the  display  calls  are  made  for  the  background,  the  terrain  object  and 
then  the  properly  ordered  ASV  object.  Swapping  display  buffers  completes  the 
loop. 

The  ASV  simulation  program  presented  here  consists  of  fifteen  separate  files 
linked,  together  with  the  graphics,  math,  and  standard  input-output  libraries, 
using  the  UNIX  make  utility.  The  program  files  and  Makefile  listings  are 
presented  in  the  appendix.  The  routines  were  created  in  a  modular  fashion  for 
ease  of  development  and  testing  and  to  assist  in  future  program  changes. 
Constants  are  grouped  into  a  single  header  file  walk.h,  for  convenient  reference 
and  modification. 

E.     SUMMARY 

This  chapter  describes  the  ASV  simulation  program.  The  first  section  is  a 
guide  for  the  operation  of  the  program.  It  details  the  use  of  the  menu  system  and 
the  operator  controls.  The  following  section  discusses  the  display  of  graphics  on 
the  IRIS-2400.  The  final  section  covers  the  organization  and  flow  of  the  main 
routine  and  its  modules. 
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V.    SIMULATION  PERFORMANCE 

This  chapter  provides  a  brief  review  of  the  performance  of  the  ASV 
simulation  program.  The  review  is  largely  subjective  and  is  based  on  the  author's 
experience  with  the  operation  of  the  simulation. 

A.    MODELING  FIDELITY 

The  image  of  the  vehicle  on  the  screen  appears  to  be  a  reasonable  likeness  of 
the  actual  ASV.  This  is  believed,  to  a  great  extent,  to  be  due  to  the  proper 
scaling  of  dimensions  of  component  parts,  based  on  available  blueprints  of  the 
ASV.  Details  such  as  the  leg  hydraulic  actuator  housings  and  the  optical 
scanning  radar,  mounted  on  the  cab  of  the  vehicle,  add  to  the  visual  effect.  The 
color  scheme  of  the  simulation  vehicle  has  been  altered  to  enhance  the  visibility  of 
the  vehicle  and  its  parts. 

The  walking  motion  of  the  model  is  very  similar  to  that  of  the  real  vehicle. 
This  observation  is  based  on  viewing  of  videotapes  produced  at  the  Ohio  State 
University.  A  notable  difference  is  that  the  simulation  model  is  perceived  to 
operate  at  a  much  slower  speed.  A  simulation  time  increment  of  1/lOOth  of  a 
second  yields  a  display  time  to  real  time  speed  ratio  of  1:30.  Operating  the 
simulation  with  a  simulation  time  increment  much  greater  than  1/lOOth  of  a 
second  to  compensate  for  this  causes  problems  related  to  the  optimum  period  and 

79 


leg  phase  modules  of  the  program.    This  leads  to  gross  errors  in  the  foot  trajectory 
planning  algorithms. 

B.  FORWARD  WAVE  TRIPOD  GAIT 

Driving  in  the  forward  wave  tripod  gait  mode  is  a  very  simple  task.  Although 
a  three-axis  joystick  would  be  prefered.  the  mouse-driven  sliding  bar  control  is 
easy  to  use  and  effective.  Switching  between  control  bars  for  forward,  lateral,  or 
rotational  control  can  be  accomplished  with  reasonable  ease. 

The  externa!  vantage  point  of  the  vehicle  causes  very  little  problem  for 
maneuvering  the  vehicle.  This  may  change  as  the  model's  speed  increases  and 
obstacles  are  introduced  into  the  environment. 

Overall,  maneuverability  of  the  ASV  in  the  forward  wave  tripod  gait  mode  is 
clearly  demonstrated  with  this  model. 

C.  FOLLOW-THE-LEADER  TRIPOD  GAIT 

The  follow-the-leader  tripod  gait  appears  to  work  especially  well  for  forward, 
straight-line  locomotion.  Turning,  however,  is  extremely  restricted.  Preliminary 
investigations  indicate  a  minimum  turning  radius  of  18  times  the  body  length, 
using  the  constrained  working  volumes  depicted  in  Figure  2.4.  This  is  far  greater 
than  expected.  An  estimated  envelope  for  turning,  based  on  initial  simulation 
trials,  is  shown  in  Figure  5.1.  .Steering  commands  falling  outside  of  this  envelope 
result    in    faults    within    the    foot    trajectory    planning    algorithms.     These   faults 
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usually  occur  within  the  first  four  degrees  of  the  turn  attempt.  Decreasing  the 
display  time  interval  extends  the  maximum  magnitude  of  the  command  envelope, 
but  only  marginally  improves  the  permitted  relative  direction  command  input. 

The  shape  of  the  steering  envelope  is  rather  unexpected,  and  as  of  yet. 
unexplained.  Factors  likely  to  have  the  greatest  influence  on  the  envelope  are  the 
geometry  of  the  leg's  constrained  working  volume  and  the  implementation  of  the 
optimum  period  and  foot  trajectory  planning  algorithms. 

Expanding  the  constrained  working  volume  to  the  full  working  volume  has  a 
remarkable  effect  on  the  maneuverability  of  the  vehicle,  while  operating  in  the 
follow-the-leader  gait  mode.  By  doing  so,  the  minimum  turning  radius  improves 
to  approximately  five  times  the  body  length.  This  indicates  a  great  potential 
advantage  in  utilizing  dynamic  stability  algorithms  for  future  gaits. 

Overall  the  follow-the-leader  gait  and  tractor-trailer  steering  appear  to  be 
successful  for  level,  relatively  obstruction-free  terrain.  Further  research  is  needed 
to  determine  the  nature  of  the  limitations  and  the  means  to  expand  the  vehicle's 
maneuverability  while  operating  in  this  mode. 
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VI.    SUMMARY  AND  CONCLUSIONS 

In  this  thesis  a  tripod  follow-the-leader  gait  class  is  introduced  for  use  by  six- 
legged  walking  vehicles.  The  class  represents  an  extension  of  previously  defined 
follow-the-leader  gaits  and  should  prove  useful  for  legged  vehicles  traveling  in 
rough  or  treacherous  terrain  conditions. 

A  new  style  of  steering  is  also  developed  for  follow-the-leader  gaits.  This 
steering  mode  exhibits  a  general  response  similar  to  that  found  in  steering  a 
wheeled  tractor-trailer  vehicle.  With  this  mode,  the  driver  is  concerned  only  with 
specifying  the  velocity  of  the  front  of  the  vehicle.  The  algorithm  ensures  that  the 
body  of  the  vehicle  follows  along  the  path  of  the  front. 

An  improved  simulation  model  constructed  to  study  the  gait  and  steering 
algorithms  is  also  presented  in  this  thesis.  The  vehicle  selected  as  a  physical 
reference  for  the  model  is  the  Adaptive  Suspension  Vehicle  (ASV).  which  is 
currently  undergoing  testing  and  development  at  the  Ohio  State  University.  The 
model  developed  is  intended  as  a  general  tool  for  analyzing  a  variety  of  walking 
control  algorithms  for  legged  vehicles. 
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A.    RESEARCH  CONTRIBUTIONS 

Previous  research  on  follow-the-leader  gaits  [Ref.  3],  has  concentrated  on  gaits 
that  are  temporally  oriented.  Since  footholds  are  used  by  the  following  legs 
immediately  after  being  abandoned  by  the  lead  leg.  this  produces  a  creeping 
motion  with  alternating  leg  and  body  movement. 

Extending  the  class  of  follow-the-leader  gaits  into  the  spatial  domain  relieves 
the  requirement  of  immediately  utilizing  a  foothold  as  soon  as  it  is  abandoned. 
This  gives  a  greater  degree  of  freedom  to  leg  movement  and  allows  the  possibility 
of  smooth,  continuous  body  motion  with  shorter  leg  stroke. 

The  nature  of  a  follow-the-leader  gait  greatly  constrains  the  maneuverability 
of  the  walking  vehicle.  The  vertical  projection  of  the  vehicle's  center  of  gravity  is 
required  to  fall  within  the  support  pattern  of  the  legs  and  is  therefore  confined  by 
the  history  of  footholds  produced  by  the  lead  legs.  The  similarity  of  this  problem 
to  that  of  a  trailer  pulled  by  a  tractor  cab  has  inspired  the  adoption  of  the  term 
"tractor-trailer"  steering.  With  tractor-trailer  style  steering,  the  driver  controls 
the  path  of  the  front  of  the  vehicle.  As  long  as  the  driver  does  not  turn  too 
sharply  (possibly  causing  a  wheeled  tractor-trailer  to  jack-knife),  the  vehicle's 
body  follows  along  this  path. 

The   selection   of  footholds   for   the   leading   legs   is   based   on   projecting   the 

relative    heading    vector    provided    by    the    operator.      The    location    of    recently 

abandoned    footholds    is    retained   within    the   control    algorithm   for   use   by   the 

middle  and  rear  legs. 

84 


The  simulation  presented  in  this  study  models  the  kinematics  of  the  ASV. 
The  model  incorporates  many  of  the  simulation  features  presented  by  Lee  [Ref.  4], 
including  omnidirectional  control,  automatic  body  altitude  and  attitude 
regulation,  leg  motion  planning,  body  deceleration,  and  filters  between  the  control 
inputs  and  reaction  which  provides  the  operator  with  the  "feel"  of  vehicle 
dynamics.    A  simplified  variation  of  constrained  working  volumes  is  also  used. 

The  simulation  program  has  a  modular  design  which  creates  a  flexible 
environment  for  studying  various  gaits  and  control  algorithms.  The  program  as 
currently  configured  has  two  modes  of  operation.  The  first  features  a  forward 
wave  tripod  gait  with  three-axis  control  for  steering  in  body  coordinates.  The 
second  mode  utilizes  the  follow-the-leader  tripod  gait  and  two-axis  control 
tractor-trailer  style  steering,  developed  in  this  study.  The  program's  displays  and 
controls  are  operated  with  a  mouse-driven  menu  package  using  a  single  mouse 
button. 

The  graphics  presentation  is  greatly  improved  over  Lee's  monochrome  line- 
drawing  representation.  Three-dimensional,  solid  body,  color  graphics  are  made 
possible  through  the  implementation  of  the  model  on  the  special  purpose  software 
and  hardware  of  the  IRIS-2400  system.  This  provides  a  notable  enhancement  of 
realism  for  the  vehicle  simulation. 
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B.     RESEARCH  EXTENSIONS 

It  has  become  clear,  through  the  work  of  developing  this  study,  that  there  are 
many  directions  in  which  future  research  could  be  pursued.  Four  major  areas  to 
be  considered  for  extension  are:  quantitative  measurement  of  the  FTL  tripod  gait 
performance,  improvement  of  program  features,  improvement  of  display  speed, 
and  expansion  of  upper  level  control  algorithms  using  artificial  intelligence. 

Developing  performance  criteria  for  the  simulated  ASV  is  critical  if  one  is  to 
effectively  use  the  program  as  an  aid  for  developing  and  evaluating  walking 
algorithms  for  the  actual  machine.  Initial  research  might  well  concentrate  on 
measuring  turning  radii,  steering  reaction  times,  stability  margins  and  basic 
parameters  of  mobility. 

As  with  any  simulation  model,  there  are  many  desired  features  which  could 
be  added  to  enhance  realism.  Perhaps  the  most  important  improvement  for  this 
type  of  mobile  vehicle  would  be  the  inclusion  of  rough  or  uneven  terrain  into  the 
model.  Provisions  were  made  in  the  development  of  this  model  for  that 
eventuality.  A  few  new  algorithms,  for  functions  such  as  estimating  the  support 
plane  beneath  the  vehicle  and  adjusting  the  constrained  working  volume  to 
conform  to  the  terrain  slope,  will  need  to  be  written.  It  should  be  possible  to 
follow  the  work  of  Lee  [Ref.  4],  at  least  initially,  in  improving  the  simulation  in 
This  direction. 

Because  the  ASV  is  designed  for  rough  terrain  locomotion,  developing  a  good 

foothold   search   algorithm   is   important.     In   addition,   the   inclusion  of  foothold 
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search  into  the  simulation  model  would  enable  the  FTL  gait  to  be  better 
evaluated  with  respect  to  other  types  of  gaits  in  various  terrain  conditions.  This 
would  quantify  the  advantages  of  reduced  foothold  probing  requirements  for  the 
FTL  gait. 

This  simulation  could  also  be  used  to  further  develop  steering  mechanisms  for 
the  ASV.  Most  notably,  the  algorithm  for  the  tractor-trailer  style  steering  uses  a 
simple  method  for  body  positioning  based  on  the  centroid  of  the  established 
footholds.  A  different  method  for  body  steering  which  minimizes  the  maximum 
leg  excursions  might  improve  the  vehicle's  turning  radius. 

Dynamic  modeling  and  supplementing  the  model's  kinematics  would  greatly 
improve  the  realism  of  vehicle  movement.  Moreover,  it  should  also  notably 
increase  mobility,  as  the  vehicle  would  be  free  to  utilize  its  leg's  full  working 
volumes.  This  should  also  lead  towards  the  development  of  a  great  number  of 
new  gaits,  which  are  dynamically,  but  not  statically,  stable. 

Graphics  techniques  can  be  improved  to  enhance  the  realism  of  the  displayed 
image.  Features  such  as  shading,  depth-cueing,  reflectivity  of  surfaces,  terrain 
definition,  and  increased  vehicle  detail  are  all  possible  using  current  state-of-the- 
art  techniques.  Higher  resolution  monitors  and  an  enlarged  number  of  bit  planes 
in  the  display  hardware  are  also  highly  desirable. 

Adding   additional   features   to   the   model   has   the   decided   disadvantage  of 

requiring   more   cpu   time   for   the   simulation.     As   the   program   now   exists,   the 

simulated   vehicle   moves    and    reacts   markedly   slower   than    the    actual   vehicle. 
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There  are.  however,  ways  to  improve  the  display  time  for  the  model.  The  prime 
means  is  of  course  to  upgrade  the  hardware,  using  newly  developed  and  more 
capable  machines.  The  code  may  also  be  streamlined  for  efficiency.  Possibly 
several  of  the  interactive  features  could  be  reduced  or  eliminated. 

Another  possibility  for  improving  display  time  is  the  replacement  of  the 
integration  routines  within  the  body  kinematics  section  of  the  program  with  an 
incremental  homogeneous  transformation  matrix  technique  used  by  Lee  [Ref.  4]. 
Integration  is  used  here  because  of  the  simplicity  of  the  technique  and  the 
author's  familiarity  with  the  IRIS-2400  special  hardware  commands  for  rotation 
and  translation.  It  may  be  that  the  homogeneous  transformation  matrix  could 
also  be  used  directly  with  the  special  hardware  to  provide  the  full  transformation 
with  fewer  trigonometric  computations.  This  possibility  has  not  been  investigated 
by  the  author. 

An  interesting  avenue  of  research  to  explore  is  to  automate  the  upper  levels  of 
the  control  hierarchy.  It  may  be  possible  to  use  an  expert  system  shell  running  on 
a  special  purpose  LISP  machine  to  provide  driving  commands  to  this  simulation. 
As  of  this  writing,  efforts  are  underway  by  others  to  establish  communications 
between  the  IRIS-2400  system  and  a  Symbolics  3675  LISP  machine. 

Extensions  to  the  work  presented   in  this  thesis  are  possible  and  will  likely 

prove  very  fruitful.    It  is  hoped  that  this  line  of  research  will  lead  to  more  efficient 

and  practical  gaits  and  control  algorithms  for  legged  walking  vehicles  in  rough 

terrain. 
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APPENDIX 
PROGRAM  LISTING 


^*^  +  **X*X***********x*^TX*x**3C3ca>:»:**T*:*x**  +  *:*3C**ai:*;**** 


This  program  is  written  for  the  iris-2400 
walk.c 


This  is  the  main  program  for  the  simulation 
Relle  Lyman  04  May    1987 

include  "gl.h" 
if  include  "device. h" 
include  "walk.h" 
^include  <stdio.h 
^include  <  math  h 

main( ) 

{ 

Object  machineobject  4    leg  7    4  .textobj,vertextobj  ,thighobj[7'[2][4|, 
actuatorobj  7  [2][4],shinobj  7    2    4  .  walker  4j,groundobject; 

*  NOTE:  this  program  uses  only  elements  1-6  of  arrays  and  vectors. 
Legs  are  numbered  to  remain  consistent  with  original  research  .    */ 

Tag  transrot    tag  4,tr   end    tag  4 '..  legmovetag  7  J4  . 
actmovetag  7    2    4    bodytagl[4l, 
thighmovetag  7    2    4  .shinmovetag  7][2][4j; 

Colorindex  wrnask  : 

int      i.j.k.n. 

program   status.  /*  desired  status  of  program:  RUN,  HALT  or  RESET  * 

selected    gait.     -""  indicates  which  tripod  gait  is  to  be  used  *  ' 

slow    flag.         /*  flag  indicating  deceleration  is  needed  */ 

warning.  *  flag  indicating  supporting  leg  is  outside  of  working  volume  */ 

leg   statusj7j;      *  status  of  leg  (supporting,  liftoff,  transfer,  placement)  */ 

static  float 

hx[7]=  {O.155..155.,0.,0.,-155.,-155.},  /*  Leg  attachment  points  */ 
hy|7j=  {0,50. ,-50. ,50. ,-50. .50. ,-50.}. 
hz[7j=  {0.23. ,23. ,23., 23. .23. ,23.}, 
14  7  =  {0.L4.-L4.L4.-L4.L4.-L4}: 

static  Angle     theta  7i  =  {0,0.0,0,0.0.0}.        /*  Leg  component  angles  * 
alpha  7j  =  { 364, 364, 364, 364, 364 ,-364. -364}. 
gamma(7]  =  {317, 317, 3 17, 31 7. 3 17. -317. -3 17}: 
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ilk.c 


*  / 


float    temp. tempi. temp2. tempo, top, bottom,  /'*  Temporary  variables  */ 
alpharad  7  .  /*  Leg  component  angles  in  radians  */ 

thetarad. 

legcoord    x  7  ,        /*  Foot  position  in  leg  coordinates  */ 
legcoord   y[7j, 
legcoord    z[7], 

azimuth, elev, roll,    /*  Body  Euler  angles  (rads)  */ 

ordered    vel    mag,    /*  Ordered  velocity  of  the  cockpit  (magnitude)  */ 
ordered   vel   dir,       /*  Ordered  velocity  of  the  cockpit  (direction)  */ 
dlj7|,  /*  Joint  variables  */ 

d2[7], 

knee[7][2;.  /*  Relative  position  of  knee  */ 

foot[7i [2] ,         /*  Relative  position  of  foot  */ 
h|4][4],  /*  Homogeneous  transformation  matrix  */ 

invh(4][4],        /*  Inverse  homogeneous  transformation  matrix  */ 
legphase[7i,        /*  Phase  of  individual  legs  */ 

rel    legphase(7j,         /*  Phase  of  individual  legs  relative  to  leg  one*/ 
period,  /*  Period  of  leg  cycle  *  j 

min    period.  /*  Minimum  allowed  period  *  ' 

tx,ty.tz:        /*  Earth  coordinates  of  body  position  */ 

vector    rot    rate.     '*  Body  rotation  rates  */ 

trans   rate.    /*  Body  translation  rates  *  / 

ordered    rate.      *  Ordered  lateral  and  longitudinal  translation  and  yaw  rates  */ 

footpos[7j,    /*  Position  of  foot  in  earth  coordinates  */ 

b   footpos|7],    /*  Position  of  foot  in  body  coordinates  */ 

fh'7],  /*  selected  footholds  (earth  coordinates)  */ 

oldfhi7 .;         /*  old  selected  footholds  (earth  coordinates)  */ 

work   vol    cwv(7];       /*  Constrained  working  volumes  */ 

plane     spe;         /*  Estimated  support  plane  */ 


/*    Initialize  the  IRIS  graphics  */ 

ginit ( )  ;  /*  standard  IRIS  graphics  initialization  */ 

doublebuffer()  ;       /*  double  buffering  mode  */ 

gconfig()  ;  *  configure  the  IRIS  (use  the  above  commands  */ 

wmask  =  (l<<getplanes())-l  ;  /*  enable  all  the  bit  planes  for  writing  */ 

/*  set  to  2**(getplanes())minus  one  */ 

/*  all  bit  planes  on  */ 

writemask(wmask)  ; 

backface(TRUE);  /*  set  backface  polygon  removal  on  */ 
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vvalk.c 

qdevice(MlDDLEMOLSE);  *  set  up  the  queue  for  the  menu  */ 

tie(MIDDLEMOl'SE.MOUSEX, MOUSEY): 

mapcolor(LTYELLO\V. 225. 225.0);  *  create  new  colors  */ 

mapcolor(WHITEl.230,230,230): 

viewport(410, 1023, 0,767)  ;  /*  set  world  view   * 

perspective(600. (614. 0/768). 0.0, 1023.0)  : 

*      make  the  ground      * 
makeground(<Lr  groundobject ); 

/*     make  the  robot       x7 

makewalker(machineobject.dl.d2,thet a, knee. gamma. alpha. transrot    tag. 
tr  end    tag. walker. leg. thighobj.actuatorobj.shinobj, 

legmovetag.thighmovetag.actmovetag.shinmovetag.tx.ty.tz.roll. 

elev.azimut  h.hx.hy,hz,l4)  ; 

'*    Initialize  the  ASY  walking  routine  parameters.  */ 
init ialize(h.invh.<fcrot    rate.&trans   rate, bordered    rate.&spe.&'period. 
leg    status. legphase.rel    legphase,  footpos.b   footpos,cwv.fh. 
oldfh.&selected    gait, bordered   vel   mag. bordered   vel   dir. 
<y.  min    period,  &:  program    st  at  us,  &:tx,&:ty,&:tz.&:  roll.  &elev.&  azimuth); 

while(TRl  E)         '*  Main  program  loop  * 

{ 

*    Input  the  driver's  commands.  */ 
driver   commandf&ordered    rate.&rot    rate.&trans   rate.«L'program    status, 
b   foot  pos.<t  period,  alpha,  gam  m  a,  thet  a.  tslow    fl  ag,  &•  roll,  &e  lev, 
&azimuth.&:tx.&ty.&tz. bordered   vel   mag. bordered   vel   dir.fh. 
(^selected    gait); 

if  (program    status  ==  HALT) 

{ 

/*  Quit  program.  */ 
break; 

} 

if  (program   status  ==  RESET) 

{ 

Reinitialize  the  ASV  walking  parameters.  */ 
initialize(h.invh.&Tot   rate.&trans   rate, bordered   rate.&spe.&period, 

leg   status. legphase.rel    legphase,  footpos,b   footpos,cwv.fh. 

oldfh.&selected    gait.irordered   vel   mag, bordered   vel   dir. 

&  min    period, &: program   stat  us,  &tx.&ty,&tz,&roll,&elev,&;  azimuth); 
} 
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/*    Calculate  the  estimated  support  plane.  " / 
j*    Future  revision  needed  for  rough  terrain.  */ 
support    plane(&spe); 


/*    Calculate  the  body  rotation  and  translation  rates.  */ 
body   rates(&rot   rate.&ztrans   rate. &spe,h,invh. bordered   rate, 
&tx,&ty,&tz,&roll,&elev.&  azimuth): 

/*    Calculate  the  constrained  working  volume  for  the  legs.  */ 
con   work   vol(cwv,b   footpos,leg   status. irwarning); 

/*    Calculate  the  optimal  period  for  walking.  */ 
optimal    periodflegphase.b   footpos.&rot    rate.&trans    rate,cwv, 
leg   status. &:period); 

t  *    Decelerate  if  necessary.  * 

decelerate(&:trans   rate.&rot   rate.&period.&slow    flag.&rmin    period); 

*    Calculate  the  phase  of  each  leg.  */ 
leg    phaseflegphase.rel    legphase.&period); 

/*    Calculate  the  new  position  for  each  foot.  */ 

foot    trajectory(legphase.&:period,leg   status, footpos,b   footpos.fh.oldfh, 
invh,h.cwv,&trans   rate.&rot    rate.&selected    gait); 


,  *    Display  the  ASV  on  the  screen.    */ 

*    This  section  computes  the  new  parameters  to  position  the  legs 
relative  to  the  body,  based  on  the  relative  position  of  the  feet. 
It  then  check  to  ensure  that  no  actuator  positions  exceed  the  limits.  */ 

/*  Convert  foot  position  to  leg  coordinates.  */ 
for(i=l;  i<5;  i+  — ) 

{ 

legcoord_x[i)  =  b_footpos;i].x  -  hx[ij; 

legcoord   y[i]  =  b   footpos'ij.y  -  hy[ij; 

legcoord   zjij  =  b   footpos[i  .z  -  hzii]; 
} 

/     The  foot  position  of  the  rear  legs  are  changed  to  compensate  for 

the  180  degree  rotation  used  in  the  leg  construction  routine.    */ 
for(i  =  5;  i<7;  L-f-  — ) 

{ 

legcoord   xlij  =  hxi   -  b   footposji |.x; 
legcoord _y[i]  =  b  footposlij.y  -  hy|i]; 
legcoord    z(i    =  b   footposii  .z  -  hz.ij; 
} 
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I*    walk.c     * 

for(i-  1:  i<7  :  i ) 

< 

generate  required  parameters  dl,d2,  theta  */ 

d2  i  =  legcoord    x[i   '5.0; 

temp=  legcoord    y[i    *  legcoord    y[i]: 

temp2  =  legcoord    zji  *legcoord    z[i]; 

dl  i  =  -(5.0*L3-sqrt(temp+temp2-L4*L4))/4.0; 

templ=5.0*L3-f-4.0*dl[i]  ; 

switch  (i) 

{ 

case  1: 
case  3: 
case  5:  tempS  =  templ*legcoord    y  i]  +  L4*legcoord    z  i  ; 

break: 
case  2: 
case  4: 
case  6:  tempS  =  templ*Iegcoord    yiil  -  L4*legcoord    z[i|; 

} 

thetarad  =    asin(temp3    (templ*templ  4-  L4*L4)); 

thetai    =  thetarad  *  573  —  0.5: 

} 

for(i  =  l  ;i<7  ;  i++)  /*  prepare  parameters  for  graphics    */ 

{  /*  update  on  all  6  legs  */ 

temp  =  L3->-dl  i    ; 

tempi  =  d2ii  *d2ii]  +  temp*temp: 

temp2  =  (Ll*Ll  -  L6*L6  -  tempi )/  (2. 0*Ll*sqrt (tempi )); 

alpharad  i  =((PI   2)-atan(d2  i  .  temp)-acos(temp2))  ; 

*  Note:  One  half  of  a  degree  has  been  added  to  all  angles  * 
alpha:i=(alpharad  i  *573^.5): 

knee  i;j0l  =  (L2*cos(alpharad[i  )^.5);  /*  relative  to  baseplate  */ 

knee  i][l]=  -( (L2*sin(alpharad  ii)-  dl[ij)+0.5); /*  relative  to  baseplate  *  ' 

foot[i][0j=  (5.0*d2[i]+.5);  /*  relative  to  baseplate  */ 

foot[i][l]=  -(5.0*L3  +  4.0*dl|i]-^.5)  ;    /*  relative  to  baseplate  */ 
top=(kneeii'  0j-foot|ij[0  ); 
bottom  =  (knee(i    1  ]-footji!|l|); 

gamma  i  —  (atan(top '  bottom)*573  +  .5)  ; 
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for  (n  =  0;  n<4;  n  +  ^)  /*    The  walker  is  updated  in  each  quadrant  *y 

{ 

editobj(thighobj|i](Oj jnj)  ;  /*  edit  each  leg  to  new    */ 

objreplacefthighmovetagjijjOJjn])  ;    ,       location 
rotate(alphaji|,'Y')  ; 
closeobj()  ; 

editobj(thighobj[i][l|[n])  ; 

objreplace(thighmovetag[ij[lj[n])  ; 
translate(0.0.0.0,dl[i])  ; 
closeobjf)  ; 

editobj(actuatorobj[i][0][n])  ; 
objreplace(actmovetag[i;[Oj[n])  ; 
rotate(alpha[i],'Y')  ; 
closeobj()  ; 

editobjfactuatorobj  i;ll]'nj)  ; 
objreplace(actmovetagji;[li[nj)  ; 
translate(d2  i  ,0.0,-L3)  ; 
closeobJO  ; 

editobj(shinobj!i]|Oj[nj)  : 

objreplace(shinmovetag[ij;Oj|n])  ; 
rotate(gamma!il,'Y')  ; 
closeobjf )  ; 

editobj(shinobj(i]|lJ(n])  ; 

objreplace(shinmovetag[i]!l][n])  ; 
translate((float)(knee[i]!0]),0.0,(float)knee[ij!l  )  : 
closeobjf)  ; 

editobjfleg  i ' [n] )  ; 

objreplaceflegmovetag  i]jn])  ; 
rotate(theta  i  ,'X')  ; 
closeobjf)  ; 
}     '*  end  quadrant  loop    */ 
}  /*  end  for  leg  loop  i=l  ...  */ 

for  (n=0;  n<4;  n+  +  ) 

{ 
editobj(machineobjectln))  ; 
objdeleteftransrot   tag[nj,tr   end    tag!n  ): 
objinsertftransrot   tag[n]); 
translate(tx,ty,tz): 
rotateffint)  (azimuth* 573), 'Z'); 
rotate  (( int)(elev*  573  ),'Y'); 
rotate((int)(roll*573),'X'); 
closeobjf)  ; 
}  /*  end  of  quadrant  loop  */ 
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set  up  the  background 
color(BLUE): 
clear(); 

*  Keep  the  viewing  relationship  constant.  */ 
perspective(600, (614.0/768), 0.0, 1023.0)  ; 
lookat(800.0-rtx,800.0+ty,550.0,tx,ty  ,-50.0.1100); 

'*    CALL  THE  GROUND      */ 

callobj(groundobject); 

/*  Display  the  ASV  in  the  correct  quadrant  configuration 
if  (azimuth  <  0.0) 
{ 
{ 
azimuth  +=  2.0  *  PI: 

} 

if  (azimuth  >  2.0  *  PI) 

{ 

azimuth  -=  2.0  *  PI; 

} 

if  (azimuth  <  0.25*PI) 

{ 

callobjfmachineobject  0]): 

} 
if  ((azimuth    >=  0.25*PI)&.'&(azimuth  <  0.75*PI)) 

{ 

callobjfmachineobject  3]); 

} 

if  ((azimuth      =  0.75*PI)&&(azimuth  <   1.25*PI)) 

{ 

callobjfmachineobject  1 2]): 

! 

if  ((azimuth    :  =  1.25*PI)&&(azimuth  <  1.75*PI)) 

{ 

callobj(machineobject!lj): 

} 

if  (azimuth  >=  1.75*PI) 

{ 

callobj(machineobject!0]); 


swapbuffers()  ; 
}  *  end  of  main  program  loop  */ 
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*  walk.c 

*    Clean  up  the  screen.    * 

color(BLACK)  ; 

clear()  ; 

swapbuffers(); 

color(BLACK); 

clear(): 
swapbuffers(): 
finishf)  : 
gexitj)  ; 

}    /*    END  OF  MAIN  PROGRAM  */ 
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This  is  the  header  file  for  the  program  walk.c. 
walk.h 


Relle  Lyman 

14  May  1987 

******************************** 

^define 

BETA                                 0.5 

^define 

DELTA   TIME               0.010 

^define 

TIME  CONSTANT   1  0.1 

f define 

TIME  CONSTANT  2  0.25 

^define 

TIME  CONSTANT  3  0.5 

^define 

FTL   GAIT                       1 

^define 

FVVD    WAVE  GAIT       2 

^define 

FORWARD                      1 

^define 

BACKWARD                   0 

^define 

END   LIFT  PHASE        0.2 

#define 

BEGIN   PLACE  PHASE  0.8 

-define 

SUPPORTING                 0 

^define 

LIFTOFF                          1 

#define 

TRANSFER    FORWARD  2 

^define 

PLACEMENT                  3 

^define 

ON                                     1 

^define 

OFF                                  0 

ftdefine 

LENGTH                           310.0 

#define  HALFLENGTH 


155.0 


^define 

FOOTLIFTHEIGHT 

40.0 

^define 

LONG   TIME 

1000000 

^define 

HO 

160.0      /*  Desired 

#define 

OUTER   LIMIT 

6.08        /*  cm/sec 

^define 

INNER   LIMIT 

1.52        /*  cm/sec 

#define 

RUN 

0 

*=define 

HALT 

1 

^define 

RESET 

2 

^define 

NORMAL 

0 

idefine 

SLOW 

1 

^define 

PI 

3.14159 

^define 

UP 

1 

^define 

DOWN 

2 

^define 

IN 

1 

^define 

OUT 

0 

•  define 

LTYELLOW 

100 

idefine 

WHITE1 

107 

^define 

TEXTCOLOR 

BLACK 

^define 

NOHIGHLIGHT 

LTYELLOW 

"  The  length  between  the  forward 

and  aft  hip  joints  */ 
*  Half  the  length  between  the  forward 

and  aft  hip  joints  * 


(cm)*/ 


(cm 

/ 


*/ 


#define  ACTIVEHIGHLIGHT       RED 
#define  INACTIVEHIGHLIGHT  YELLOW 
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*    walk.h  *, 

#define  Ll  20.0 

#define  L2  102.0 

^define  L3  24.0 

f define  L4  32.0 

#define  L6  30.0 


struct  mag    in    xvz       /*  magnitude  along  x,  y,  and  z  axes  */ 

{ 

float  x,y,z; 

}; 

typedef  struct  mag    in    xyz  vector; 

struct  plane   coefficients     /*  plane  coefficients  */ 

{ 

float  a.b.c.d: 

}; 

typedef  struct  plane   coefficients  plane; 

tvpedef  struct 

{ 

float    min. 
max. 
center: 

}    dimensions; 

typedef  struct 

{" 

dimensions    x, 

y- 

z; 
}    work   vol; 


typedef  struct 

{ 

int  Ieft,right,top,bottom,x0,y0.xl,yl,x2,y2; 

char  *text0.*textl.*text2; 
}    menubox; 
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/xXXXXXXX*X**XXXX*X.XXXX±*:xxXXXX*X#.*Xx*x*XXXX->****x>***-****r**xx*XXXX 

This  is  a  function  for  the  iris  2400  program  walk.c. 
in  it  .c 


Relle  Lyman  27  Apr  1987 


itinclude  "gl.h" 
^include  "device. h" 
^include  "walk.h" 

init iahze(h.invh,rot    rate. trans   rate. ordered   rate, spe, period. leg   status, 
legphase.rel    legphase.footpos.b   footpos.cwv,fh,oldfh. selected    gait, 
ordered    vel    mag. ordered    vel   dir, program    status, tx,ty.tz. roll, elev. azimuth) 

This  function  computes  the  body  rotation  and  translation  rates.  * 


vector  *rot    rate.  '*  rotation  rate  */ 

*trans    rate,  *  translation  rate  */ 

*ordered    rate.  *  ordered  x  translation,  y  translation, 

and  z  rotation  rates    */ 

fit  7  .  *  selected  footholds  (earth  coord.)  */ 

oldfh  7  .  *  old  selected  footholds  (earth  coord.)  */ 

footpos;7  .  *  position  of  the  foot  in  earth  coord.  * 

b   footpos  7  ;  *   position  of  the  foot  in  body  coord.  */ 

plane    *spe;  /*  support  plane  in  earth  coord  */' 

work    vol  cwv  7  :  /*  constrained  working  volume  */ 

float    h[4jj4 ■,  *  homogeneous  transformation  matrix  * 

invh[4  [4;,       /*  inverse  of  transformation  matrix  */ 
legphase  7!,      /*  phase  of  the  phase  */ 
rel    legphase  7l,  /*  phase  of  the  leg  relative  to  leg  one  * 
*  period.  /*  body  support  period  */ 

*tx,*ty,*tz,        /*  position  of  body  in  earth  coordinates  +/ 
*roll.*elev.*azimuth,  /*  body  euler  angles  */ 

*ordered   vel    mag.   ■'*  ordered  velocity  of  the  steering  pt  (magnitude)* 
*ordered    vel   dir:     /*  ordered  velocity  of  the  steering  pt  (direction)* 

int       leg    status|7|.  /*  status  of  the  leg  *  ' 

*program    status.  *  desired  status  of  program  */ 

^selected    gait:  /*  type  of  tripod  gait  to  be  used  *  ' 

{ 

int      i.j; 

Hoat    modulus   one();  /*  modulus  one  function  */ 
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/*    init.c 


/*  initialize  the  transformation  matrix  *, 


h|0 

[0] 

=  1.0; 

h|0 

1 

=  0.0; 

h:0 

2 

=  0.0; 

h(0 

i  o  I 

H 

=  0.0; 

hH 

[o] 

=  0.0; 

hll 

[11 

=  1.0; 

hU 

[2] 

=  0.0; 

hjl 

i  o  i 
•> 

=  0.0; 

h  2 

[o] 

-  0.0; 

h  2 

ill 

=  0.0; 

h[2 

I2) 

=  1.0; 

h;2 

=  HO; 

h  3 

0 

=  0.0; 

h  3 

;lj 

=  0.0; 

h  3 

[2] 

=  0.0: 

h  3 

3 

1.0; 

/  *  initial  height  of  the  center  of  the  body  *  / 


/*  initialize  the  inverse  transformation  matrix  */ 
for  (i=0;  i<3;  i++) 

{ 

for(j=0;j<3;j  +  +  ) 

I 

invh[i][j]  =  h[jj(i]; 

} 

invh[3][i]  =  0.0; 

invh[i![3]  =  -(hf0](ij*h;0]l3]  +  h[l][i]*h(l][3i  + 
h[2j[i]*h[2][3  ): 


/*  initialize  the  body  rotation  and  translation  rates  *  / 

rot    rate->x  =  0.0; 

rot   rate->y  =  0.0: 

rot    rate->z  =  0.0; 

trans   rate->x  =  0.0 

trans   rate->y  =  0.0 

trans   rate->z  =  0.0 


I*  initialize  the  commanded  body  rates  */ 
ordered   rate->x  =  0.0;   '*  translation  */ 
ordered   rate->y  =  0.0;    '*  translation  */ 
ordered   rate->z  =  0.0;  /*  rotation  */ 

*  period  =  LONG    TIME; 

♦selected   gait  =  FWD    WAVE   GAIT; 
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*  initialize  the  relative  leg  phase 

rel    legphase;  l:   =  0.0; 


rel  legphasei2 

rel  Iegphasej3 

rel  legphase[4 

rel  legphase[5 

rel  legphase[6 


=  0.5; 
=  BETA; 

BETA-0.5; 
-  2*BETA  -  1.0; 
=  modulus   one(2*BETA  -  0.5] 


/*  initialize  the  leg  status  and  phase  */ 
for  (i=l:  i-7:  i-  •  ) 

{ 

leg   statusli]  =  SUPPORTING; 
legphase  i    =  rel    legphase  i  ; 


initialize  the  constrained  working  volume  for  each  leg 


cwv  1  ] 

.x.min 

95.0; 

cwv  1 

.x.max       = 

215.0; 

cwv   1 

.x. center  = 

155.0; 

c  w  \    1 

.y.min       = 

60.0; 

cwv  1 

.  \   max 

131.0; 

cwv    1 

.y. center  = 

95.0; 

cwv    1 

.z.min 

-240.0; 

cwv   1 

.z.max 

-  80.0; 

cwv  1 

z. center  = 

-160.0; 

cwv  21 

x.min 

95.0; 

cwv[2j 

.x.max       = 

215.0 

cwv  2] 

.x. center  = 

155.0; 

cwv  2 

.y.min       = 

-131.0; 

cwv  2 

.y.max       = 

-  60.0; 

cwv  2 

.y. center  = 

-  95.0; 

cwv  2 

.z.min       = 

-240.0; 

cwv  2 

.z.max 

-  80.0; 

cwv  2 

.z. center 

-160.0; 

CWV    o 

.x.min       = 

-  60.0: 

CWV    3 

.x.max 

60.0; 

cwv  ."'. 

.x. center  - 

0.0; 

cw\  ;; 

.y.min 

60.0; 

cwv  3 

y.max       = 

131.0 

cwv  [3 

.y. center  = 

95.0; 

cwv  ;j< 

.z.min       = 

-240.0; 

cwv|3 

.z.max       = 

-  80.0; 

cw\  .". 

.z. center  = 

-160.0; 
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CWVJ4; 

.x.min       = 

-  60.0; 

cwv;4; 

.x.max       = 

60.0: 

cwv|4 

.x. center  = 

0.0; 

cwv|4' 

.y.min       = 

-131.0; 

cwv[4i 

.y.max       = 

-  60.0; 

cwv  4; 

.y. center  = 

-  95.0; 

cwv  4 

.z.min       = 

-240.0; 

cwv[4J 

.z.max       = 

-  80.0; 

cwv  4j 

.z. center  = 

-160.0; 

cwv  [5] 

x.min       = 

-215.0; 

cwv  [5: 

.x.max       = 

-  95.0; 

cwv  5 

.x. center  = 

-155.0: 

cwv|5i 

.y.min       = 

60.0; 

cwv|5 

.y.max       = 

131.0 

cwv|5j 

.y. center  = 

95.0; 

cwv  5 

.z.min 

-240.0; 

cwv|5 

z.max       = 

-  80.0; 

cwv  5 

.z. center  = 

-160.0; 

cwv[6j. x.min  =  -215.0; 
cwv|6j. x.max  =  -  95.0; 
cwv[6].x. center  =  -155.0; 

cwv  6  .y.min  =  -131.0; 
cwv6  y.max  =  -  60.0; 
cwv  6i.y. center  =  -  95.0; 

cwvj6!. z.min  =  -240.0; 
cwv[6!. z.max  =  -  80.0; 
cwv[6|.z. center  =  -160.0; 

/*  initialize  the  selected  foothold  positions  * 
fh[l].x  =  cwv[l].x. center  +  LENGTH/12.0; 
fh[2].x  =  cwv[2].x. center-  LENGTH   12.0; 
fh[S].x  =  cwv[3].x  center  -  LENGTH   12.0; 
fh  4!.x  =  cwv|4j.x. center  +  LENGTH   12.0; 
fh[5].x  =  cwv[5j.x.  center  +  LENGTH   12.0; 
fh[6].x  -  cwv[6j.x. center-  LENGTH/12.0; 

for  (i=l;i< 7;i  —  — ) 

{ 
fhjij.y  =  cwv[ij.y. center; 
fhlil.z  =  0.0; 
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initialize  the  earth  relative  foot  positions 
for  (i=l;i<7:H  -  ) 

{ 

footposjij.x  =  fh[i].x; 
footposji.y  =  fhlij.y; 
footpos[i].z  —  fhli  .z; 
} 

*  initialize  the  old  selected  foothold  positions  * 

for  (i=l:i<7;i-  +  ) 

i 
oldfh  i  .x  =  fhlii.x  -  LENGTH/3.0; 

oldfh  ij.y  =  cwvli    v. center: 

oldfhjij.z  =  0.0; 

} 

i  *  initialize  the  body  relative  foot  positions  * 

for  (i=l;i<7;i+  +  ) 

{ 

b  footpos[i].x  =  cwv  i   x. center; 

b  footpos[i].y  =  cwv  ij.y. center; 

b  footpos|i].z  =  cwv  i  .z. center; 


initialize  the  estimated  support  plane  */ 
spe-    a  =  0.0; 
spe-:>b  =  0.0; 
spe->c  =  1.0: 
spe->d  =  0.0; 

T  initialize  the  ordered  velocity  of  the  steering  point  */ 
^ordered   vel    mag  —  0.0; 
*ordered   vel   dir  =  0.0; 

initialize  the  body  attitude  and  position  */ 
'roll  =  0.0; 
*elev  =  0.0; 
'azimuth  =  0.0; 
*tx  =  0.0; 
*ty  =  0.0; 
*tz  =  HO; 

initialize  desired  program  status  * 
*  program    status  =  RUN; 

]      *  end  of  initialize 
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*:**x*:r***:r*********:K*****xx******************  A********************** 


This  is  a  function  for  the  iris  2400  program  walk.c. 
driver. c 


Relle  Lyman  13  May  1987 

x*******************************************************************/ 

^include  "gl.h" 
^include  "device  h" 
^include  "walk.h" 
*  include  <stdio.h> 
#include  <math.h> 

menubox     box[j  =  { 
0.0. 0,0, 0,0. 0,0. 0.0. "not", "used", "here". 

100, 200, 670.525, 120. 567. 120, 597. 120, 627,  "G  AIT".  "W  A  VE","FWD", 
200, 300, 670. 525. 220. 567. 220, 597, 220.627, "ATTITUDE", "AND", "ALTITUDE", 
300,400,670,525.320.567,320,597,320.627,"  "."RESET","  ", 
100, 200. 525, 380.120.422. 120. 452. 120. 482. "GAIT"."  ","FTL", 
200, 300, 525, 380, 220.422, 220.452, 220, 482, "REPORT","  "."STATUS", 
300, 400, 525, 380.320.422. 320, 452, 320. 482, "PROGRAM","  "."EXIT", 

100. 200. 31 0,230. 120. 250. 120, 270. 120. 290, "REVERSE". "FOR  WARD". "TRANSLATE' 
100. 200, 230, 150. 120. 170. 120, 190, 120. 210, "RIGHT". "LEFT", "TRANSLATE". 
100. 200. 150. 70. 120. 90. 120. 110, 120. 130. "RIGHT". "LEFT", "ROT  ATE". 

100.200.310.230.120.250.120.270.120.290."  "."  "."  ", 
100,200,230.150.120.170.120,190.120.210."  "."  "."  ", 
100,200,150.70.120,90,120.110,120,130,"  "."  "."  "}: 

driver   command(ordered    rate. rot    rate. trans   rate. program   status, 
b   footpos, period, alpha, gamma. theta. slow   flag, roll, elev. 
azimuth. tx.ty.tz. ordered   vel   mag. ordered   vel   dir.fh, selected    gait) 

/*    This  function  inputs  the  driver's  commands  using  a  menu  and 
the  mouse.  * 

vector  *ordered   rate,   ;*  ordered  x  translation,  y  translation,  and  z  rotation  rates  */ 
*rot    rate.  /*  actual  rotation  rate  vector  */ 

*trans   rate,        /*  actual  translation  rate  vector  */' 
b   footpos[7;,       /     position  of  foot  in  body  coordinates  */ 
fh[7j;  /*  selected  footholds  (in  earth  coordinates)  */ 

int      *program   status.  /*  desired  status  of  the  program  RUN/HALT 'RESET  */ 
*selected    gait.    /*  desired  tripod  gait  * 
*slow    flag;  /     flag  indicating  deceleration  is  required  */ 

float  *period,  /*  body  support  period  *  ' 

*tx.*ty,*tz,        /*  body  translation  distance  (Earth  coord)  */ 
*  azimuth, *elev,*roll,  /*  body  Euler  angles     * 
*ordered   vel   mag,  /*  ordered  cockpit  velocity  (magnitude)  */ 
*ordered   vel   dir:  /*  ordered  cockpit  velocity  (direction)  */ 
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+    driver. c    *, 

Angle  alpha  7  .  ■  *  thigh  angle 

gamma  7  .  *  bhin  angle  * 

t hetai 7  :  *  leg  lateral  angle 

{ 

Device        dummy. x.y; 

static  int  buttonflag  =  UP, pick, potentialpick.mainmenupick, submenu, slidebar; 

in  t  i : 

float  barvalue; 

static  float    time; 

char  str   orx  100], str  ory!l00!,str   orzlOO], 

str   trx!l00],str_try'100j,str_rrz  100  ,str   timejlOO]; 


pushmatrixf); 
pushviewport(); 

viewport  (0.500.0.767): 
onho2(0. 0,500. 0.0. 0.767.0): 

color(CYAN);        '*  screen  background  color  * 
clear( ): 

'*  Display  simulation  time  on  top  of  screen 
color(TEXTCOLOR); 
time  +=  DELTA   TIME; 

sprintffstr   time, "simulation  time  ^c8.3f".time); 
cmov-2i(l05.700); 
charstr(str   time): 

if  (qtest()  ==  MIDDLEMOUSE)     '*  Button  just  pressed  or  released  * 

{ 
qread(i'dummy ): 
qread(&x); 
qread(&y); 

if  (buttonflag  —  =  DOWN)       *  Button  was  just  released    * 

{ 

buttonflag  =  UP; 

if  (potentialpick  ==  0) 

{ 

*  No  change  */ 

} 
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driver. c    * 

else  if  (potentialpick  <  7)     /*  Main  menu  chosen  * 

{ 

mainmenupick  =  potentialpick; 
pick  =  0; 

pick  =  potentialpick; 
potentialpick  =  0; 

} 

else  /*    submenu    chosen  */ 

{ 

pick  =  potentialpick;    /*  no  change  to  main  menu  pick  * / 
potentialpick  =  0; 
} 

} 

else  /*  Button  was  just  pressed.  */ 

{ 

buttonflag  =  DOWN; 

} 

}       *  end  of  qtest  *  ' 

if  (buttonflag  DOWN)        /*    Find  the  box  over  which  the 

cursor  lies  for  highlighting.  */ 

{ 

x  =  getvaluator(MOUSEX); 
y  =  getvaluator(MOUSEY); 

potentialpick  =  0; 

for  (i=l:i<7;i-i-  +  )     /*  Check  the  main  menu.  */ 

{ 

if  (x  <  boxiij. right  &&  x  >  box[i  .left  &:& 
y  <  boxlil.top    &&  y  >  boxii. bottom) 

{ 
potentialpick  =  i; 

} 
\ 
if  (submenu  --  1)    /*  Check  submenu  #1.    */ 

{ 

for  (i=7;i<10:i++) 

{ 

if  (x  <  boxjij. right  &&  x  >  box[i|.left  && 

v  <  boxiij. top    &&  y  >  boxjij. bottom) 

{ 
potentialpick  =  i; 

I 

} 
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driver. c 

if  JMilmienu    ==  2)      *  Check  submenu  #1. 

for  (i     10;i<  13;i  +  +  ) 

1 

if  (x  <  box  i  .right  &:&  x  >  boxjij.left  &;& 
v  <  boxiii.top    &&  v  >  boxiii. bottom) 

{ 

potentialpick  =  i; 

} 
} 
I 

} 

else     /*  button  is  up  *  / 

{ 
potentialpick  =  0; 

\ 

'*  Display  the  menu.  */ 

for  (i=l;i<7;i  — ) 

{ 
if  (i  ==  potentialpick) 

{ 
color(ACTIVEHIGHLIGHT); 

} 
else  if  (i    -=  mainmenupick) 

{ 
color(INACTIVEHIGHLIGHT); 

} 
else 

\ 

color(NOHIGHLIGHT); 

i 
i 

rectfifbox  i  .left,  boxji]. bottom,  box(i). right,  box(ii.top); 
color(TEXTCOLOR); 

recti(box[i].left,  boxjij. bottom,  box'i). right,  box:ij.top); 
cmov'2i(boxji).x0,box[i].y0); 
charstr(box[ij.textO); 
cmov2i(boxiij.xl,box[ij.y  1); 
charstr(box(ij.textl); 
cmov2i(boxjij.x2,box[i].y2); 
charstr(box|i|.text2): 
\ 
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driver. c    * 

if  (submenu  -       1)       /*    Display  submenu  *1.    */ 

{ 
for  (i=7;i<10;i++) 

I 
if  (i  ==  potentialpick) 


{ 


color(ACTIVEHIGHLIGHT); 


else  if  (i  ==  pick) 

{ 
color(INACTIVEHIGHLIGHT); 

} 
else 

{ 
color(NOHIGHLIGHT); 

} 

reotfifbox  i  .left,  boxji.. bottom,  box  i  .right,  boxT.top) 
color(TEXTCOLOR); 

rectifbox  i    left.  box!i!.  bottom,  box  i    right,  boxiiitop); 

cmov2i(box|i  .xO,box!i;.yO): 

charstr(box(ij.textO); 

cmov2i(box!i;.xl,box|ij.yl); 

charstr(box[ii.textl); 

cmov  2i  (box  [i;.x2,  boxji  j.y2); 

c  h  arsl  r  ( box  I  i  i .  tex  1 2 ) : 


color(  WHITE); 

rectfi(200.70, 400,370); 

color(BLACK); 

recti(200.70,3O0.15O): 

recti(300,70,400,150); 

recti(200,150.300,230) 

recti(300,150.400,230) 

recti(200, 230, 300,310) 

recti(300,230.400,310) 

recti(200,310.300,370) 

recti(300,310,400,370); 

color(RED); 

cmov2i(205,350); 

charstr("ORDERED") 

cmov2i(205,330); 

charstr("RATE"); 

cmov2i(305.350); 

charstrfACTUAL"): 

cmov2i(305.330); 

charstrC'RATE"); 


Draw  LED  gages.  */ 
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driver. c 

Display  the  parameter  values.  * 
sprintf(str   orx,"%7.2P, ordered    rate-  >x); 

spnntfjstr   ory  "%1 .2V , ordered    rate-    y); 

sprintf(str   orz."t,:c7.2f". ordered    rate-  -z); 
sprintf(str_trx,"%7.2P,.trans_rate->x); 

sprintf(str   try. "%7 .2r\ trans   rate->y): 

sprintf(str   rrz,"%7.2P',rot   rate-  >z); 

cmov2i(205.270); 
charstr(str   or.x): 
cmov2i(205,190); 
charstrfstr   ory); 
cmov2i(205.110); 
charstrfstr   orz): 
cmov2i(305^270); 
charstrfstr    trx): 
cmov2i(305?190); 
charstrfstr    try); 
cmov2i(30.^.110); 
charstrfstr    rrz); 

} 

if  (submenu  —-  2)         *    Display  submenu  $2.    */ 

{ 
for  (i=10;i<13:i-i — ) 

{ 

if  (i  ==  potentialpick) 

{ 
color(ACTIVEHIGHLIGHT); 

} 
else  if  (i  ==  pick) 

{ 
color(INACTIVEHIGHLIGHT); 


color(NOHIGHLIGHT); 

} 

rectfifbox  i    left,  boxii  .bottom,  box  ij. right,  box  i  .top) 
color(TEXTCOLOR); 

recti(box!i].left,  boxjij. bottom,  box  1    right,  boxii]. top); 
cmov2i(box[i;.x0.box[i].yO); 
charstr(box ij.textO); 
cmov2i(box[ij.xl,box|ij.yl): 
c  h  arstrf  box  ji].  text  1); 
cmov2i(box[i].x2,box[i].y2); 
charstr(boxi].text2); 
} 
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driver. c 


color(WHITE); 
rectfi(200, 70,400, 370); 
color(BLACK); 
recti(200,70, 300,150); 
recti(300, 70,400,150); 
recti(200, 150,300, 230); 
recti(3()0. 150.400,230); 
recti(200,230,300,310); 
recti(300. 230.400.310); 
recti(200. 310,300,370); 
recti(300, 310.400,370); 
color(RED): 
cmov2i(205,350); 
charstr("ORDERED"): 
cmov2i(205,330); 
charstr("ANGLE"); 
cmov2i(305,350); 
charstr("ACTUAL"); 
cmov2i(305,330); 
charstr("ANGLE"); 


Draw  LED  gages. 


} 


switch  (pick) 

{ 
case  1:        submenu  =  1; 

'selected   gait  =  FWD   WAVE  GAIT; 

break; 


case  2:        submenu 
break; 


2; 


case  3:        submenu  =  3; 

*program   status  =  RESET: 
break; 

case  4:        submenu  =  4; 

joystickftrans   rate, rot   rate, ordered    vel    mag. ordered    vel    dir,&buttonflag) 
steering    conv(ordered    rate, ordered    vel    mag. ordered    vel    dir, 

azimuth. tx,ty,fh); 
*  selected    gait  =  FTL   GAIT; 
break; 
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driver. c 

case  5:        submenu  =  5; 

status    report(ordered    rate. trans    rate, rot    rate, 
b    footpos.period.alpha,gamma.theta. 
slow    flag,roll.elev,azimuth,lx.t\  . 

tz); 
break; 

case  6:  *    exit  *  / 

'program    status       HALT; 
break; 

case  7;        bar(-200.0. 200. O.&slidebar.&barvalue, trans   rate->x) 
if  (slidebar  ==  IN) 

{ 

ordered    rate->x  =  barvalue; 

} 
break; 

case  8:        bar(- 100.0.100  0,&slidebar.<Lbarvalue, trans   rate->y) 
if  (slidebar  =  =  IN) 

{ 
ordered    rate->y  =  barvalue; 

break; 

case  9:         bar(-1.0,l  .0.&.'slidebar,&'barvalue,rot    rate->z); 
if  (slidebar  -  =  IN) 

i 

ordered    rate->z  —  barvalue; 

} 
break; 

case  10;       /*  Future  expansion  * 
break; 

case  11:        '*  Future  expansion  */ 
break: 

case  12:        '*  Future  expansion  */ 
break; 

default:       color(BLACK): 
} 

popviewport) ); 
popmatrix( ): 

}     *  end  of  driver   command  */ 
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*    driver. c    * 

bar(minval.  maxval.slidebar,barvalue,  actual  value) 
float  minval.  maxval.*barvalue,actualvalue; 
int      "slidebar: 

{ 

register  i; 
char        str  20]; 
int  x.y; 

static  int     barlevel; 

/*    Draw  the  sliding  bar.  *, 

cursoff(); 

color(BLACK); 

rectfi(9,69.90.690): 

color(RED); 

recti(10.70.30.670); 

for  (i=0;i<5;i-i — ) 

{ 

move2i(30.70  +  i*150): 
draw2i(40,70  +  i*  150); 
cmov2i(34.73  -  1*150); 
sprintffstr.  "%6. IP1, minval  +  i*(maxval-minval)/4.0); 

charstr(str); 

} 

curson(); 

/*  Check  the  location  of  the  cursor.    If  it  is  inside  the 

sliding  bar,  then  calculate  the  value  for  its  position.  */ 
x  =  getvaluator(MOUSEX): 
y  =  getvaluatorfMOl'SEY); 
if  ( 10  <  x  &&  x  <  SO  &&  70  <  y  &&  y  <  670) 

{ 

barlevel  =  y; 

*slidebar  =  IN; 

*barvalue  =  minval  +  (maxval  -  minval)*(y  -  70)/600.0; 

} 
else 

{ 

*slidebar  -  OUT; 

} 

/*  Draw  the  bar  showing  the  actual  level.  */ 

color(RED); 

rectff  15. 0,70. 0.25.0,  (370. 0+600.0*actualvalue/(maxval-minval))): 

/*  Draw  the  bar  showing  the  ordered  value.  */ 
color(  YELLOW); 

rectifll, 70, 29, barlevel); 
}    /*  end  of  bar  */ 
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joystick(trans   rate. rot    rate. ordered    vel    mag. ordered    vel   dir.buttonflag) 

vector     *trans   rate.       *  translation  rates  of  the  center  of  gravity  in  body  coordinates  * 
*rot    rate;         *  body  Euler  angle  rotation  rates  */ 

float       'ordered    vel    mag,  /  *  ordered  velocity  of  cockpit  (magnitude)  * 
"ordered    vel   dir;  /*  ordered  velocity  of  cockpit  (direction)  */ 

int  *buttonflag;    /*  indicator  for  middle  mouse  button  */ 

{ 

int  x.y.i; 

float  vx.vy,      /*    velocity  of  cockpit  in  body  coordinates  */ 

magn.dir:     *  magnitude  and  direction  of  cockpit  velocity  vector  */ 


*  Display  the  steering  box.  * 
color(BLUE); 

recti(  100.80,400,380): 

*  Display  the  grid  */ 
for  (i=l;i<15;i++) 

{ 

move2(90.0+i*20. 0,80.0): 
draw2(90  0-i*20. 0.380.0); 

} 

for  (i=l;i<  15;i+  +  ) 

{ 
move2(100.0,80.0+i*20.0); 
draw2(400.0,80.0+i*20.0); 

} 

*  Display  instructions.    */ 
cmov2i(  110,65); 

charstr("Hold  the  middle  button  down"); 
cmov2i(  110,50); 

charstr("to  control  the  joystick"); 

/*    Display  the  current  velocity  of  the  cockpit.    */ 

vx  =  trans   rate->x; 

vy  =  rot_rate->z  *  HALFLENGTH  +  trans   rate->y; 

magn  =  sqrt(vx*vx  -~  vy*vy); 

dir    =  atan2(vy,vx); 

if  (vx    -  -  0.0) 

{ 

dir  -  0.0; 

} 

linewidth(5); 

color(  YELLOW); 

move2(250. 0,80.0); 
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*    driver. c 

if  (vx  ==  0.0) 

{ 

dir  -  0.0; 

> 

linewidth(5): 

color(YELLOW); 

move2(250. 0,80.0); 

draw  2((  250. 0-400. 0*dir).  (80. 0+magn*3.0)); 

/*    Check  the  location  of  the  cursor.  */ 
x  =  getvaluator(MOUSEX); 
y  =  getvaluator(MOUSEY); 
if  (*buttonflag  ==  DOWN) 

{ 

if  (100  <  x  &&  x  <  400  &&  80  <  v  &&:  v  <  380) 

{ 

* ordered   vel   mag  =  (y-80)  '3.0; 
*  ordered  _vel_dir  =  (250-x)/ 400.0; 

} 

i 
I 

*    Display  the  ordered  velocity  of  the  cockpit.  */ 
linewidth(S): 
color(RED); 
move2(250. 0.80.0); 

draw2((250.0  -  400  0  *  "ordered    vel   dir), (80.0  +  *ordered   vel   mag  *  3.0)); 
linewidt  h(  1 ); 
}     /*  end  of  joystick  */ 
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:»*tr.)t»**KTX*X**TJ'tM"X>T'"tX!cJ"K»x!«X**X):s*»l*t 


This  is  a  function  for  the  iris2400  program  walk.c. 
steering,  c 


Relle  Lyman  26  Mar  1987 


=  mrlude  "gl.h" 
=  include  "device. h" 
include  "walk.h" 
£  include  <stdio.h> 
^include  •  math.h 


steering   conv(ordered    rate, ordered    vel    mag. ordered    vel   dir, azimuth. tx,ty.fh) 

This  function  calculates  converts  ordered  head  velocity  to 
ordered  body  translation  and  rotation  rates.    */ 

float      ^ordered    vel    mag.      *  ordered  velocity  of  the  cockpit  (magnitude)  */ 
*ordered    vel    dir.  *  ordered  velocity  of  the  cockpit  (direction)  */ 

'azimuth.  '   body  azimuth  angle  (radians) 

'i\.  "ty:  '  current  position  of  the  body's  center  of 

gravity  (in  earth  coordinates)  */ 

vector     *ordered    rate.         *  ordered  forward  and  lateral  translation 

rates  and  azimuth  angle  rate    * 
fh  7  ;  *  selected  foothold  (in  earth  coordinates)  */ 


float  hx.hy,  *  current  head  (cockpit)  position  (earth  coord.)*/ 

dhx.dhy.  *  desired  head  position  (earth  coord.)  *; 

fhcen    x.fhcen  y,  .,  *  foothold  centroid  (earth  coord.)  */ 

dcgx.dcgy,  /*  desired  center  of  gravity  (earth  coord.)  */ 

dazimuth.  I*  desired  azimuth  angle  (earth  coord.)  */ 

diffazm;  f*  difference  between  desired  and  current  azimuth  */ 

vector   desired    rate;     /*  desired  earth  translation  rates  and  azimuth 
angle  rate  * 
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steering   conv 


'*    Note:  This  module  uses  a  level  terrain  assumption.  */' 

/*    Calculate  current  head  position  (earth  coordinates), 
hx  =  *tx  -  HALFLENGTH  "  cos(  *azimuth); 
hy  =  *ty  -  HALFLENGTH  *  sin(  "azimuth): 

/*    Calculate  the  desired  head  position  (earth  coord).  */ 

dhx  =  hx  -j-  DELTA   TIME  *  "ordered   vel   mag  *  cos(  "ordered    vel   dir  +  "azimuth] 

dhy  =  hy  +  DELTA   TIME  *  "ordered   vel   mag  *  sin(  *ordered   vel   dir  +  *azimuth) 

/*    Calculate  the  foothold  centroid.  (Forward  gaits  only)  */ 
fhcenx  -  (fli[3].x+fhj4].x+fh[5].x+fh[6].x)/4.0; 
fhceny  =  (fh[3].y+fh'4].y+fhj5].y+fh[6].y)/4.0; 

/*    Calculate  the  desired  azimuth  angle.  */ 
dazimuth  =  atan2((dhy-fhcen   y ),(dhx-fhcen   x)): 
diffazm  =  dazimuth  -  *azimuth: 

/*    Adjust  the  difference  to  a  value  between  pi  and  -pi.    */ 
if  (diffazm  <  -3.14159) 

{ 
diffazm  +=  6.2831853; 

} 

if  (diffazm   >  3.14159) 

< 
diffazm  -=  6.2831853: 

\ 

I*    Calculate  the  desired  center  of  gravity.  *  ' 
dcgx  =  dhx  -  HALFLENGTH  "  cos(dazimuth); 
dcgy  =  dhy  -  HALFLENGTH  *  sin(dazimuth); 

*    Calculate  the  desired  rates  (earth  coord.).  */ 
desired   rate. x  -  (dcgx  -  *tx)/DELTA_TIME; 
desired  rate.y  -  (dcgy  -  *ty)/DELTA_TIME; 
desired    rate.z  =  diffazm /DELTA   TIME; 

/*    Convert  to  body  translation  and  rotation  rates.  */ 
ordered   rate->x  -~  cos(  *azimuth)  *  desired   rate.x 

-  sin(  "azimuth)  "  desired   rate.y; 
ordered    rate->y  —  cos(  *azimuth)  *  desired    rate.y 

-  sin(  "azimuth)  *  desired   rate.x; 
ordered    rate->z  =  desired    rate.z: 

}    /*  end  of  steering   conv    */' 
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,    »     .     ,    »«»x****»:»xx**xX********XXX***XX>r,-r*-.     t     .    -.     ,     ,     ,     ,xxxx»x»XX*X 

This  is  a  routine  for  the  iris-2400    program  walk.c. 
status,  c 


This  routine  creates  a  status  report  to  be  displayed 
on  the  viewing  screen  beneath  the  ASV. 
Relle  Lyman  27  Mar    1987 

^include  "gl.h" 
^include  "device. h" 
include  "walk.h" 

status   report  (ordered    rate, trans   rate, rot    rate,b   footpos, period. alpha, gamma. 
theta,slow   flag, roll, elev,  azimuth,  tx.ty,tz) 

int         *slow    flag;      *  flag  indicating  deceleration  is  needed  *  ' 

Angle     theta|7],  *  leg  component  angles  * 

alpha  7  . 
gamrna(7]; 

float     *  period.  *  period  of  leg  cycle 

*tx.*ty.*tz,        /*  position  of  body  in  earth  coordinates  * 
*roll.*elev.*azimuth:  /*  body  Euler  angles  * 

vector    *rot    rate,        /'*  body  rotation  rates  * 
*trans   rate,  /*  body  translation  rates  *  ' 

*  ordered    rate.     *  ordered  lateral  and  longitudinal  and  >  aw  rates  */ 
b  footpos  7  :         *  foot  position  in  body  coordinates  * 

{ 

int      l.k: 

char   str   fpx  7    100  .str Jpy[7][lOO|,str_fpz!7][lOO], 
str  orx  100  .str_ory(lOO:,str_orzllOO], 
str_trx;100].str_try[lOO],str_trz(lOO], 
str  rrx;]00],str_rry[l00),str_rrzjl00j, 
str_alpha!7]|lO0j,str_gamma[7j[l00],str_theta:7j|100], 
str   period  |l00j,str_slow|  100], 
str   tx[lOO],str_tyilbo|,  str_tzll00], 
str  roll|T00j,str   azimuthllOO  ,str   elevllOO  ; 
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st.atus.c 


spnn 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 
sprin 


tffstr   orx."c77.2f", ordered    rate->x); 

tf(str   ory."(77.2f", ordered    rate->y); 

tffstr   otz"%7  2P\  ordered    rate-:>z): 

tffstr   trx."ac7.2f',trans_rate->x); 

tffstr    try. "9? 7.2P. trans   rate- >y); 

tffstr   trz,"%7.2f".trans_rate->z); 

tf(str_rrx,"%7.2f",rot_rate->x); 

tf(str_rry,"%7.2f",rot   rate->y); 

tffstr rrz."%7.2f". rot   rate-  >z); 

tffstr    period. MCx9.5f".*period); 

tffstr  Jx."c^7.2fVtx): 

tf(str_ty."%7.2f".*ty); 

tf(str_tz,"%7.2f",*tz); 

tffstr   roll, "c~o7d",((int)  (*roll  *  573.0))); 

tffstr   azimuth. ,K77d",((int)  (*azimuth  *  573.0))); 

tffstr  _elev,"%7d"l((int)  (*elev  *  573.0))): 


for  (k=l;k<7;k++) 

{ 
sprintffstr   fpx  k  ,',(H>7.2f".b   footposlkj.x); 
sprintffstr   fpv  k],"%7.2f",b_footpos[k].y); 
sprintf(str_fpz(k;,"c>67.2f".b_footposjk    z): 
sprintffstr   alpha(k;,"%7d".alphaik^; 
sprintffstr   gamma  I  k  ,"%7d",gammaiki): 
sprintf(str_thetajk],"%7d",thetajk]); 

} 


pushmatrixf); 

viewportfO. 400, 0,767); 

ortho2(0. 0,400. 0.0. 0.767.0): 

colorfBLACK); 

rectfiflO. 10. 400.370); 

color(YELLOW); 

rectfi(20. 20. 390,360); 

colorfBLACK); 

cmov2i(220,340); 

charstrf'X"): 

cmov2i(280.340); 

charstrf'Y"); 

cmov2i(340.340): 

charstrf'Z"): 

cmov2i(30,325); 

charstrf'ordered   rate"); 
cmov2i(30,310);  ~ 
charstrf'trans   rate"); 
cmov2i(30,295J; 
charstrf'rot   rate"); 
cmov2i(30,280); 
charstrf  "posit  ion" ) ; 
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status. c 


cmov2i 
rharstr 
cmov2i 
charstr 
cmov2i 
charstr 

cmov2i 
charstr 
cmov2i 
charstr 

cmov2i 

charstr 


210,265): 

"ROLL"): 

260,265); 

"ELEV."): 

310.265): 

"AZIMUTH"); 

30.250); 

"current  attitude" 
30.235); 
"ordered  attitude' 

30,210); 

"period"): 


if(*slow    flag  ==  SLOW)       *  moving  too  fast  * 

{ 

cmov2i(750,220); 
color(RED): 
charstr("TOO  FAST"): 
color(BLACK); 
} 


cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 


30.185): 

".x  ft  pes  (1-3)"); 

110.170): 

"(4-6)"): 

30,155); 

"y  ft  pos  (1-3)"); 

110.140); 

"(4-6)"); 

30.125); 

"z  ft  pos  (1-3)"); 

110.110); 

"(4-6)"); 

30.95); 

"ALPHA      (1-3)" 

110,80); 

"(4-6)"); 

30,65); 

"GAMMA       (1-3) 

110.50); 

"(4-6)"): 

30.35): 

"THETA       (1-3)" 

110.20); 

"(4-6)"): 
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status. c  */ 


cmov2i 
charstr 
cmo\2i 
charstr 
cmo\2i 
charstr 

cmov2i 
charstr 
cmov2i 

charstr 
cmov2i 
charstr 

cmo\2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 

cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 

cmov2i 
charstr 

cmov2i 
charstr 
cmov2i 
charstr 
cmo\2i 
charstr 


180.325); 
str  orx); 
240,325); 
str  ory); 
300,325); 
str   orz): 

180,310); 
str   trx); 
240,310); 
str   try  ): 
300,310); 
str   trz); 

180,295); 
str   rrx); 
240,295); 

str    rry); 
300.295): 
str   rrz): 

180,280); 
strtx); 

240,280); 

strty); 
300,280): 
str   tz); 

180,210); 
str   period); 

180,250); 
str  roll); 
240,250): 
str  elev): 
300.250): 
str   azimuth) 
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status,  c 


for  (i  ~  l:i-  4;i- 


k=  110- 
cmov'2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmo\2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 
charstr 
cmov2i 

charstrfs 
cmov2i 

charstr(s 


i*70; 
k.  185); 
str   fpxli] 
k.170); 
str   fpx  i- 
k,155); 
str  fpyjij 
k.140); 


str_fpy[i+3 

k.125); 

str   fpzji]); 

k.flO); 

str  fpz  i  +  3 

k.95); 

str    alpha  i 

k,80); 

str   alpha  i- 

k,65); 

str   gamma 

k,50); 

str   gamma 

k.35); 

tr   thetaji]) 

k.20); 

tr   theta .[-t 


>  ): 


3); 


popmatrix(); 
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This  is  a  function  for  the  iris  2400  program  walk.c. 
support. o 


Relle  Lyman  21  Aug  1986 

******************************** 


^include  "gl.h" 
^include  "device. h'' 
^include  "walk.h" 
^include  <stdio.h> 
^include  <math.h> 

support    plane(spe) 

This  module  will  compute  a  new  estimated  support  plane  based  on 
the  position  of  the  supporting  legs.    As  a  temporary  measure  it 
is  assumed  the  support  plane  is  flat  and  at  "sea  level"  (i.e. 
z  =  0  ).    The  equation  for  the  plane  is  Ax  — By+Cz  +  D=0.    */ 

plane    *spe;         *  estimated  support  plane  in  earth  coordinates  * 


spe->a  =  0.0: 
spe-  >b  =  0.0; 
spe->c  =  1.0; 
spe->d  =  0.0; 
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******************************************##x#**!(:***x*************** 


This  is  a  function  for  the  ins  2400  program  walk.r. 
body    rates. c 


Relle  Lyman  19  Apr  1987 

XX»-***x*x*********x*XX*****X* 


^include  "gl.h" 
^include  "device. h" 
^include  "walk.h" 
-include  <  stdio.h 
^include  <  math.h 

body    ratesfrot    rate. trans   rate. spe.h,invh, ordered    rate.tx,ty.tz, 
roll. elev, azimuth) 

This  function  computes  the  body  rotation  and  translation  rates.  */ 


vector  *rot    rate.  *  rotation  rate  *  ' 

*trans   rate.  *  translation  rate  * 

*ordered    rate:  /*  ordered  x  translation,  y  translation. 


plane    *spe;  *  support  plane  in  earth  coord  */ 

float    h  4] [4],  ./*  homogeneous  transformation  matrix  * 

invhl4] i 4 j ,     I*  inverse  transformation  matrix  *t 
*tx,*ty,*tz,  /*  position  of  body  in  earth  coordinates  * 
*  roll. *elev. "azimuth:     *  body  Euler  angles  */ 

int        i.j: 

float     eta,  *  body  plane  attitude  wrt  earth  plane  */ 

eta   d,         *  desired  body  plane  attitude  * 
height,        *  distance  form  CG  to  est.  support  plane  */ 
height    d.     *  desired  height  *j 

gamma.         *  angle  between  desired  and  present  body  unit  normal  vectors  *, 
kx,  *  x  component  of  rotation  unit  vector  in  body  coord  */ 

ky,  /*  y  component  of  rotation  unit  vector  in  body  coord  */ 

ka.  /*  control  law  gain  */ 

a,b,c,       /*  body  plane  desired  unit  normal  in  body  coord  */ 
length,      /*  rotation  vector  normalizing  factor 
celev,selev.telev,        '*  sine. cosine, tangent  of  elev 
croll,sroll,cazim,sazim,  /    sin  and  cos  of  roll  and  azimuth 
m; 


*  / 
*  / 


plane     spb;  /*  support  plane  in  body  coordinates  * 

vector    db    unit    norm.   /*  desired  body  plane  unit  normal  in  earth  coordinates  */ 
trans    rate   e,    '*  Translation  rates  in  earth  coordinates 
rot    rate   e;        *  Rotation  in  bod}-  Euler  rates  */ 
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/*    body    rates. c    */ 

*  Calculate  the  body  plane  attitude,  (level  ground  assumption!)* 
eta  =  0.0; 

*  Calculate  desired  body  plane  attitude  (level  ground  assumption!)*/ 
eta  d  =  eta; 

/*  Calculate  the  desired  body  clearance,  (level  ground  assumption!)*/ 
heightd  =  HO; 

*  Calculate  the  support  plane  coefficients  in  body  coordinates.  */ 

/*     [spb]  T  =  [spej  T  *  h 

spb.a  =  spe->a  *  h[0][0]  +  spe->b  *  h[l ][0'  +  spe->c  *  h[2j[0]  +  spe->d  *  h[3][0]; 
spb.b  =  spe->a  *  h[0][l]  +  spe->b  *  h[l][l  +  spe->c  *  h:2  |1  +  spe->d  *  h[3j[l]; 
spb.c  =  spe->a  *  h[0][2]  +  spe->b  *  h[l][2]  +  spe->c  *  h[2][2j  -  spe- :>d  '  h[S]'[2]; 
spb.d  =  spe->a  *  h[0j[3j  +  spe->b  *  h[l][3j  +  spe->c  *  h  2  [3    +  spe->d  *  h  3    3  ; 

/  *  Height  of  body  CG  above  support  plane  */ 
height  =  spb.d; 

Calculate  desired  unit  normal  for  the  body  plane  in  earth  coord.  */ 

m  =  sqrt(spe-  -a  *  spe->a  -+-  spe->b  *  spe->b); 
'*    check  for  division  by  zero  */ 
if  (m>0.0) 

{ 
db   unit    norm.x  =  spe->a  x  sin(eta   d)  /   m; 
db   unit    norm.y  =  spe->b  *  sin(eta  d)  /  m; 


db   unit    norm.x  =  0.0; 
db   unit   norm.v  =  0.0; 

} 

db    unit    norm.z  =  cos(eta   d); 
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*    body    rat ps. c 

Transform  the  desired  unit  normal  vector  of  the  body  plane 

from  earth  to  body  coordinates.    ja,b.c:T  =  invhr*b    unit    norm 
Note:    invhr  is  the  inverse  of  the  rotational  transformation 

submatrix    (first  three  rows  and  columns  of  h).  * 
a  -  invh|0  |0l*db   unit   norm.x   r  invh:Oj[l]*db   unit   norm.y  + 

invh  0    2i*db    unit    norm.z: 
b  =  invhjlj[0  *db   unit    norm.x  —  invhiljll  *db   unit   norm.y  + 

invh  1    2l*db   unit    norm.z: 
c  =  invh  2   0  *db   unit   norm.x  +  invh'2][l]*db   unit    norm.y  — 

invh  2  [2]*db   unit    norm.z; 

*  Control  law  yielding  an  exponential  response  * 

*  d  gamma/dt  =  -ka  *  gamma  */ 
ka  =  1  TIME  CONSTANT   1; 

gamma  =  acos(c): 

length  =  sqrt(a*a  +  b*b); 

if  (length  <  .00001) 

{ 

kx  --  0: 
kv  -  0: 

} 
else 

{ 

components  of  rotation  unit  vector  in  body  coordinates 

kx  =  -b   length; 

kv  =    a    length; 
} 

*  Calculate  rotation  and  translation  rates  * 
trans  rate->z  =  -ka  *  (height    d  -  height): 
rot    rate->x  =  -ka  *  kx  *  gamma; 

rot   rate->\  =  -ka  *  ky  *  gamma: 

*  Rate  =  dt  *  acceleration  —  old  rate 

trans  rate->x  =  DELTA   TIME  *  (ordered   rate->x  -  trans   rate->x)/ 

TIME  CONST  ANT  2  +  trans_rate->x; 
trans  rate->y  =  DELTA   TIME  *  (ordered   rate->y  -  trans  rate->y)/ 

TIME  CONSTANT  3  -  translate- >y; 
rot    rate-  >z  =  DELTA   TIME  *  (ordered   rate->z  -  rot   rate->z)/ 

TIME   CONSTANT   3  +  rot  "rate- >z; 

*  Conversion  to  Earth  coordinate  translation  rates.  */ 

croll  =  cos(*roll); 
sroll  =  sin(*roll); 
telev  =  tan(*elev); 
celev  =  cos(*elev); 
selev  =  sin(*elev); 
cazim  =  cos(*azimuth); 
sazim  =  sin(*azimuth); 
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*    body   rates. c  * 

trans   rate   e.x  =  trans   rate- :x  *  croll'cazim  + 

trans   rate->y  *  (cazim*sroll*selev  -  sazim*celev) 
trans   rate->z  *  (sazim*selev  -  cazim*sroll*celev) 

trans  rate   e.y  =  trans   rate->x  *  crolTsazim  4- 

trans   rate->y  *  (sazim*sroll*selev  —  cazim*celev) 
trans   rate->z  *  (cazim*selev  —  sazim*sroll*ce]ev) 

trans   rate   e.z  =  -trans   rate-  >x  *  sroll  - 
trans   rate->y  *  croll*selev  — 
trans   rate->z  *  cazim*celev; 

/*  Conversion  to  body  Euler  rates  */ 

rot    rate   e.x  =  rot    rate->x  +  rot    rate-  >y  *  telev  *  sroll  -+- 

rot    rate->z  *  telev  *  croll; 
rot    rate   e.y  =  rot    rate->y  *  croll  -  rot    rate->z  *  sroll; 
rot    rate   e.z  =  rot    rate->y  *  sroll  /  celev  + 

rot    rate->z  *  croll  /  celev: 

/*  Integration  routine  */ 

*tx  +=  trans   rate   e.x  *  DELTA   TIME; 

*ty  +=  trans   rate   e.y  *  DELTA   TIME; 

*tz  +=  trans   rate   e.z  *  DELTA    TIME; 

*roll  +=  rotrate  je.x  *  DELTA   TIME: 

*elev  +=  rotrate   e.y  *  DELTA    TIME; 

*  azimuth  +=  rot    rate   e.z  *  DELTA   TIME; 


*  Update  the  H  matrix 

croll  =  cos(*roll); 
sroll  =  sin(*roll); 
telev  =  tan(*elev): 
celev  =  cos(*elev): 
selev  =  sin(*ele\); 
cazim  =  cos(*azimuth): 
sazim  =  sin(*azimuth): 


/ 
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bod v    rates. c  * 


hjOj 

[0  j 

h[0] 

[1] 

h  i) 

[2] 

h|0j 

I'  i 
■  > 

h[l] 

[0] 

h[l] 

1 

h[l] 

2 

h[l] 

:'. 

h[2j 

jo] 

h[2l 

i 

h(2; 

2 

h[2' 

:: 

h[3: 

(i 

h  3 

l 

h(3 

(21 

h[3 

•> 

-  croll   cazim: 

=  cazim*sroll*selev  -  sazim*cele\ : 

=  sazim*selev  +  cazim*sroll*celev; 

=  *tx; 

=  sazim*croll: 

=  cazim*celev  +  sazim*sroll*selev; 

=  sazim*sroll*celev  —  cazim*selev; 

=  *ty; 

=  -sroll; 

=  crolPselev; 

=  croll*celev; 

=  *tz; 

=  0.0; 

=  0.0: 

=  0.0; 

=  1.0; 


*  inverse  homogeneous  transform  matrix  *  / 
for  (i=0;  i<3;  i ) 


for(j=0;j-  3;j++) 

< 

invh  i  j     =  hljjji); 


invh  3][i]  =  0.0; 

invh  i](S    =  -(hj0][ij*h[0][3l 


hill|i.*hilll3|  -  h|2||i!*h|2||3| 


invhi3||3    ==   10: 
*  end  of  body    rates  * 
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This  is  a  function  for  the  iris  2400  program  walk.c. 
con    work    vol.c 


at*:***:*:***:*.*:*:************* 


Relle  Lyman 


19  Apr  1987 


**^***^*****X***********************^^***   +   *********T*TX*X***   +   *****      / 


^include  "gl.h" 
f  include  "device. h" 
f  include  "walk.h" 
^include  <stdio.h> 
f include  <math.h> 

con    work    vol(cwv.b   footpos.leg    status, warning) 

This  module  will  compute  a  new  constrained  working  volume  for 
improved  stability  on  rough  terrain.    Currently  all  values  are 
set  for  a  perfectly  flat  support  plane.    Dimensions  are 
expressed  in  cm.    */ 

work    vol    cwv[7j;    /  *  constrained  working  volume  in  body  coordinates 


vector      b   footpos  7  ;   /,:  foot  position  in  body  coordinates  */ 

int  leg   status[7],/*  status  of  leg  (supporting,  liftoff,  transfer. 

placement)  *  ' 
*warning;  *  flag  indicating  supporting  leg  is  outside  of 

the  working  volume  */ 


*  / 


int  i; 


^warning  =  OFF: 

for  (i=l;i<7;i++)    /*  check  each  leg  */ 

{ 

if  (leg  status  i    ==  SUPPORTING) 


if  ((b  footposjij.x  <  cwvjij.x.min)j| 
(b  footposli  .x  >  cwv[i].x.max)| 
(b  footposjij.y  <  cwv[i].y.min)|| 
(b  footpos'ij.y  >  cwv[i].y.max)|| 
(b  footposjij.z  <  cwv(ii.z.min)! ' 
(b   footposlii.z  >  cwv[ij.z.max))     /*  outside  limits  */ 


{ 


*warning  =  ON; 


} 
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con    work    vol.c    * , 

if  (^warning  ==  ON) 

{ 

pushmatrixQ; 

pushviewport  (); 
viewport(0. 130. 0,80); 
ortho2(0. 0.130. 0.0. 0.80.0); 
color(RED); 
rectfi(10,10.130.70); 
color(BLACK); 
cmov2i(  10.30); 
charstr("  OUTSIDE  CWV"] 
popviewport(); 
popmatrix(); 
\ 


} 
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**********:i:*:*::k  ***:?:*****************:********:■:**  ******  »".  *.■    •    •    ■ 

This  is  a  function  for  the  iris2400  program  walk.c. 
opt    period. c 


Relle  Lyman  29  Apr  1987 


:*****************^*^***x**ap************3F***»:*^:*»:*x******^c*     ' 

/ 


^include  "gl.h" 
^include  "device. h" 
^include  "walk.h" 
^include  <stdio.h  • 
^include  <math.h> 


optimal   period(legphase,b  footpos,rot   rate, trans   rate, cwv, leg   status, period) 

*    This  function  computes  the  optimal  period  for  walking.    */ 

vector  *rot    rate.         /'*  body  rotation  rate  *,' 
*trans   rate.       '*  body  translation  rate  */ 
b   footposi?  ;     '*  position  of  foot  in  body  coordinates  */ 

work    vol    cwv  7  :         /*  constrained  working  volume  *  ' 

float    legphasei7  .        *  phase  of  leg 

*period:  /*  body  support  period 

int       leg   status[7j;  /*  status  of  leg    0  =  supporting    */ 

{ 
vector  b   footvel;        /*  foot  velocity  */ 

float    fx.fy.fz,         /*  foot  position  */ 

tmin.  /*  minimum  temporal  kinetic  margin  */ 

tx,ty,tz,  /*  temporal  kinetic  margins  */ 

d,  /*  distance  to  cwv  limit  */ 

speed,  /*  magnitude  of  body  velocity  */ 

fs   period,        /*  forward  support  period  */ 

bs   period,        /*  backward  support  period  */ 
m in   fs   period,  /*  minimum  forward  support  period  */ 
min    bs   period,  /*  minimum  backward  support  period  */ 

fs    phase,  /*  forward  support  phase  */ 

bs   phase.  /*  backward  support  phase  */ 

mvx,mvy,mvz; 

int       l. 

minleg: 

static  int    gait  =  FOR WARD;   f*    Wave  gait  direction  */' 
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*    optimal    period 

:    Initialize  variable> 
tmin  -  LONG    TIME; 
nun   fs    period  =  LONG    TIME: 
nun    bs   period  -  LONG   TIME; 

For  each  leg  compute  the  maximum  instantaneous  support  period.  * 
for  (i=l;  i*^  7;  i++) 

{ 

*    Support  check 
if  (leg  status  i,  ==  SUPPORTING) 

{ 

/*    Compute  foot  velocity.  */ 

b   footvel.x  =  -(trans   rate->x)^(rot    rate->z  *  b   footposjij.y) 

-(rot    rate->y  *  b   footpos  1  .z); 
b   footvel.y  =  -(trans   rate- >y)-(rot   rate->z  *  b   footposlij.x) 

-f  (rot    rate->x  *  b   footpos(i.z); 
b   footvel.z  =  -(trans   rate->z)  +  (rot    rate-">y  *  b   footpos  1    x) 

-(rot    rate-    x  *  b    footpos  1  .y); 

*    Check  to  see  if  foot  is  in  cwv. 
fx  =  b   footpos  ij.x; 
fy  =  b   footpos  i|.y; 
fz  =  b   footpos|i  .z: 
if  ((fx-    cwv  i    x  nun)    (fx      cwvi  .x.max)j 

(fy    cwv  i    y.min)  ;  (fy      cwv  i;.y  max) 

(fz-    cwv  i.z.min)|   (fz  >  cwv(ij.z.max))      /*  outside  cwv  */ 

< 
tmin  =  0.1: 

} 
else 

{ 

/*    Compute  distance  to  x  limit  and  the  temporal 

kinetic  margin  in  the  x  direction.  */ 

if  (b   footvel.x  >  0.0) 

{ 
d  =    cwv  ij.x. max  -  fx; 
tx  =  d  /  b   footvel.x: 

} 

else  if  (bfootvel.x  <  0.0) 

< 
d  =  fx  -    cwv  ij.x.min; 

tx  =  d      -b   footvel.x; 

} 
else 

{ 
tx  -  LONG   TIME; 

} 
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optimal   period 


/*  Check  for  minimum  value.     / 
if  (tx-  tmin) 


tmin  =  tx; 
} 

/*    Compute  distance  to  y  limit  and  the  temporal 

kinetic  margin  in  the  y  direction.  */ 
if  (bfootvel.y  >  0.0) 

{ 

d  =    cwvjij.y.max  -  fy; 

ty  =  d  /  b   footvel.y; 

} 

else  if  (b  footvel.v  <  0.0) 

{ 

d  =  fy  -    cwvlij.y.min; 

ty  =  d  /  -b   footvel.y; 

} 
else 

i 
ty  =  LONG   TIME; 

} 

/*  Check  for  minimum  value.  */ 

if  (ty<  tmin) 

{ 
tmin  =  ty; 

} 

/*    Compute  distance  to  z  limit  and  the  temporal 

kinetic  margin  in  the  z  direction.  */ 
if  (bfootvel.z  >  0.0) 

{ 
d  =    cwv[i].z.max  -  fz; 

tz  =  d  /  b  footvel.z; 

} 

else  if  (bfootvel.z  <  0.0) 

{ 
d  —  fz  -    cwv[i].z.min; 
tz  =  d  /  -b   footvel.z; 

} 
else 

{ 
tz  =  LONG   TIME; 

} 

/*  Check  for  minimum  value.  */ 

if  (tz<tmin) 

{ 

tmin  =  tz; 

} 
}    /*  end  inside  cwv  */ 
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optimal    period 

('ompute  the  support  phase  for  forward  and  backward  gaits.  * 
fs    phase  =  legphase  i     BETA: 
bs   phase       (BETA  -  legphaseli:)   BETA; 

/*    Compute  support  period.    */' 

fs   period  -  (tmin-0.1)/(  1.0  -  fs   phase); 

bs   period  =  (tmin-0.1) /(1.0  -  bs   phase); 

/*    Find  the  minimum  support  period.  */ 
if  (fs   period  <  min   fs   period) 

{ 

min    fs    period  =  fs    period; 


if  (bs    period  <  min    bs    period) 

{ 
min    bs   period  =  bs   period; 

}    ( 
}       *    end  support  check 
}       *  end  leg  loop  * 

'*    Choose  gait.  *  / 
speed       sqrt(trans   rate->x  *  trans   rate->x  + 

trans   rate->y  *  trans   rate->y); 
if  ((speed      OUTER _LIMIT)&&(trans_rate->x  >  INNER   LIMIT)) 

{ 

gait   -   FORWARD: 

> 

else  if  ((speed<OUTER_LIMIT)&&(trans_rate->x<  -INNER    LIMIT)) 

{ 

gait  -  BACKWARD; 

} 
else 

< 

/*    No  gait  change.  */ 

} 

if  (gait  ==  FORWARD) 

{ 

*  period  =  min   fs   period; 

} 

else 

{ 

'period  =  min    bs   period; 

I 
}    /*  end  optimal   period  */ 
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****************************************** ************************** 


This  is  a  function  for  the  iris  2400  program  walk.c. 
decelerate. c 


Relle  Lyman  04  May  1987 


e  *  *  *  *  *     ■ 


^include  "gl.h" 
#include  "device. h" 
^include  "walk.h" 
^include  •  stdio.h> 
^include  <math.h> 


decelerate(rot   rate, trans   rate, period, slow   flag.min   period) 

/  *    This  function  computes  the  foot  transfer  rate  and  slows  the 
vehicle  if  the  maximum  rate  is  exceeded.  */ 


vector  *rot   rate,       /*  body  rotation  rate  */ 

*trans   rate:      /*  body  translation  rate  */ 

float     *  period.  *  optimal  period  for  the  leg  cycle  */ 

*min    period:    /*  minimum  leg  period  */ 

int       *slow    flag:     /*  flag  indicating  vehicle  has  been  slowed.  */ 


int         i,j: 

float     transfer   time;  /*  time  from  liftoff  to  placement  */ 


if(*period  <  *min   period)  /*  slow  down  */ 

{ 

*slow    flag  =  SLOW; 
*period  =  *min   period; 

trans   rate->x  *=  .95; 
trans   rate->y  *=  .7; 
trans   rate->z  *=  .95; 
rot    rate->x     *=  .95; 
rot   rate->v     *=  .95; 
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decelerate. c 

*  display  warning  on  screen 
pushmatrix(): 
pushviewport  (). 
viewport(200, 330. 0.80); 
ortho2(200. 0.330. 0,0. 0.80.0); 
color)  YELLOW); 
rectfi(210,10,330.70): 
color(BLACK); 
cmov2i(210,30); 
charstrf"  DECELERATION 
popviewport  (); 
popmatrix(); 

} 
else 

{ 

*slow   flag  =  NORMAL; 


end  of  decelerate* 
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/ 


This  is  a  function  for  the  iris2400  program  walk.c. 
leg    phase. c 


Relle  Lyman  24  Aug  1986 

#include  "gl.h" 
^include  "device. h" 
^include  "walk.h" 
^include  <stdio.h  > 
^include  <math.h> 

leg    phasef  legphase,  rel    legphase.  period) 

/*    This  function  computes  the  phase  for  each  leg.    */' 

float     legphase1?,,  I*    phase  of  leg    */ 

rel   legphasej7|,  /*    relative  phase  of  leg    */ 
*period:  '*    body  support  period       */ 

{ 

static  float      bodyphase  =  0.0;  /*  kinematic  phase  of  body  * 

float        modulus   one();     /*    modulus  one  function  */ 

int  i: 


/*  Calculate  new  body  phase.  */ 

bodyphase  -  modulus   one(bodyphase  +  DELTA   TIME/(*period) 

/*    Calculate  new  phase  for  each  leg.  (modi)  */ 
for  (i=  1 ;  i< 7 ;  i-  — ) 

{ 

legphase  i    =  modulus   one(bodyphase  -  rel    legphase(ij); 

> 


}    /*    end  of  legphase    */ 
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This  is  a  function  for  the  iris'2400  program  walk.c. 
ft    traj.c 


Relle  L\man  19  Apr  1987 


include  "gl.h" 
^include  "device. h" 
include  "walk.h" 
^include  <stdio.h> 
^include  <math.h> 

foot    trajectory(legphase. period. leg   status, footpos.b   footpos.fh. 
oldfh,invh,h,cwv, trans   rate, rot   rate, selected    gait) 

*    This  function  calculates  the  trajectory  for  each  foot.  */ 

float      legphase  7  .        *  Phase  of  individual  leg  */ 

*  period,  *  Optimal  period  */ 

h!4'|4j,  /*  Homogeneous  transformation  matrix  */ 

invh|4|J4  :  *  Inverse  transformation  matrix        */ 

vector     footpos'7'. 

b   footpos  7  . 

fh  7  .  *  Foothold  selection  (earth  coordinates)  */ 

oldfh  7  .  *  Old  foothold  selection  (earth  coordinates)  */ 

"trans   rate,      /*  Body  translation  rate  */ 
*rot    rate:  *  Body  rotation  rate  */ 

work    vol     cwv  7  ; 

int        leg   status  7  .     *  State  of  individual  leg  */ 
*selected    gait;  /*  Desired  tripod  gait  */ 

{ 

float  trans    time.  /*  Time  required  to  go  from  leg  liftoff  to  leg  touchdown    * 

end    lift    phase,      /*  Point  in  transfer  phase  where  liftoff  ends    */ 
begin    place   phase,    /*  Point  in  transfer  phase  where  placement  begins  * 
trans    phase;  /*  Leg  transfer  phase  */ 

static  int     liftoff  flag  7;=OFF.      *  Indicates  first  time  entering  the 
subroutine  in  the  current  leg  cycle.  *  / 
transfer   flag[7  =OFF, 
place   flag  7:  =  OFF; 

static  vector    d    footpos[7  ;     /*  Desired  foot  position  */ 

int       i;  /*  Leg  number  */ 
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*    foot    trajectory  */ 

*    Calculate  the  time  required  to  move  a  leg  from  liftoff  to 
touchdown.    (Transfer  time)      */ 
trans   time  =  (1.0-  BETA)  *   _ABS(* period): 

/*    Calculate  phase  points  marking  change  of  transfer  mode 
(direction).    Note:    Modify  later  to  account  for  transfer 
time.    */ 

end    lift   phase  =  0.2; 

begin    place   phase  =  0.8; 

/*    For  each  leg    */ 
for  (i=l:  i<7;  i++) 

{ 

/*  Calculate  transfer  phase.  */ 
/*  (lift  =  0.0  place  =  1.0)  */ 
if  (*  period  <  0) 

{ 

trans   phase  =  (1.0  -    legphase  i\) ,  (1.0  -  BETA); 

) 
else 

{ 

transphase  =  (  legphaseji   -  BETA)  '(1.0  -  BETA); 

} 

Find  the  leg  transfer  state.  *  ' 
if  (trans    phase  <  0.0) 

{ 

leg   status, i    =  SUPPORTING: 

support    trajectory  (liftoff   flag, place   flag. transfer   flag.footpos, 
b   footpos.invh.i); 

} 

else  if  (trans    phase  <  end    lift    phase) 

{ 

leg   status|i|  =  LIFTOFF; 

lift    trajectory(liftoff  flag, place   flag, transfer   flag,footpos, 

b   footpos,invh,&trans   time,&trans   phase, &end   lift   phase, i); 

} 

else  if  (trans   phase  <  begin   place   phase) 

{ 

legstatusiij  -  TRANSFER   FORWARD; 
transfer   trajectory  (liftoff  flag, place   flag, transfer   flag,footpos, 
b   footpos,h,invh.<L'trans   time.&trans   phase, 
&begin   place   phase, i,cwv, trans   rate, rot   rate,fh.oldfh, 
period, selected   gait); 
} 
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/*    foot    trajectory  */ 

else  '    end    place    phase  <    trans    phase  <    1.0 

{ 

leg   status  i    -  PLACEMENT; 

placement    trajectory(liftoff   flag. place    flag. transfer   flag.footpos, 
b   footpos.invh.&trans    time.&trans    phase, i); 
} 
I 

}     '*  end  of  foot    trajectory  */ 
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foot    trajectory  */ 


lift    trajPctory(liftoff   flag. place    flag. transfer    flag.footpos.b   footpos. 
invh, trans   time, trans    phase. end    lift    phase. leg    number) 

/*    This  function  calculates  the  trajectory  of  the  foot  while  it  is 

being  lifted  from  the  ground.    It  is  called  from  foot    trajectory ().*' 

vector      footpos! 7j,        *    Present  foot  position  in  earth  coordinates  */ 
b   footpos[7];  /*    Present  foot  position  in  body  coordinates    */ 

float       *trans   time, 

*end    lift    phase, 
*trans    phase, 
invh  4];4  ;       /*  Inverse  homogeneous  transformation  matrix  */ 

int  liftoff  flagj?  ,  /*  Indicates  the  first  time  entering  subroutine 

in  the  current  leg  cycle.  */ 
transfer   flag|7  , 
place    flag[7], 
leg    number: 

{ 

float     lift    time: 

int         i: 

static  vector     d    footpos  7  :         Desired  foot  position 
in  earth  coordinates  * 


i  =  leg   number: 

/*    Calculate  the  desired  footposition.  */ 
if  (  liftoff  flagjii  !=  ON) 

{ 

d   footpos[i].z  =   footposlij.z  +  FOOTLIFTHEIGHT; 

liftoff  flag  I  i]  =  ON; 

transfer   flagjij  =  OFF; 

place_flag[il  =  OFF; 
} 


(Calculate  the  time  required  to  reach  the  desired  height 
from  the  present  foot  position.    */ 
lift    time  =  *trans   time  *  (*end   lift   phase-  *trans   phase) 
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*    foot    trajectory  * 

!  *    Calculate  the  new  foot  position.  (Earth  Coordinates) 
if  (DELTA   TIME  <  lifttime) 

{ 

footposjij.z  — =  (d   footposji  .z  -    footpos  ij.z) 
*  DELTATIME  /  lift    time: 

} 

else  /*    Last  increment  of  time 

{ 

footposjii.z  =  d   footpos  ij.z; 


1    Transform  to  body  coodinates.    */ 
f       bfootpos  i']T  =  invh  *  [footpos[i]]T    */ 
transform    point(b   footpos, invh. footpos. i); 


}        *  end  of  lift   trajectory 
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*    fool    trajectory  * 

transfer    trajectory  (liftoff  flag. place    flag. transfer    flag.footpos. 

b    footpos.h.invh.  trans   time. trans    phase. begin    place    phase, 
leg    number. cwv, trans   rate. rot    rate, fh.oldfh, period. selected    gait) 

I*    This  function  calculates  the  trajectory  of  the  foot  during  the 
phase  in  which  the  foot  is  transfered  forward.    The  function 
is  called  from  foot    trajectory ().*/ 

vector     footpos  7],     /*    Present  foot  position  in  earth  coordinates  */ 
b   footpos|7J,  /*    Present  foot  position  in  body  coordinates    */ 
fh[7],  /*    Selected  footholds  (earth  coordinates)  */ 

oldfh ! 7] ,        /*    Old  selected  footholds  (earth  coordinates)  */ 
*trans   rate,    /*    Body  translation  rate  */ 
*rot    rate;       /*    Body  rotation  rate  */ 

work    vol    cwv[7|;         /*  Constrained  working  volume  in  body  coordinates  */ 

float        *trans    time, 

*begin    place    phase, 
*trans    phase. 

*  period.  /*  Optimum  period  of  gait  */ 

h  4  [4],  /*  Homogeneous  transformation  matrix  */ 

invh  4]  4];       /*  Inverse  transformation  matrix  */ 

int  liftoff  flag[7],  /*  Indicates  the  first  time  entering  subroutine 

in  current  leg  cycle  */ 
transfer    flag[7], 
place    flag  7  . 

"selected    gait,      *  Desired  tripod  gait  */ 
leg    number; 

{ 

float     trans   fwd  time.  /*  Time  remaining  in  the  transfer  forward  phase  */ 

vx,vy,  /*  Velocity  of  cockpit  in  body  coordinates  */ 
rel   hd,  /*  Relative  heading  of  cockpit  velocity  */ 

proj   dist.         /*  Projected  distance  forward  for  new  footholds  */ 
min   time;  /*  Minimum  time  to  reach  any  cwv  limit  */ 

int        i; 

vector     cwv   velocity,     /'*  Instantaneous  velocity  of  the  center  of 
the  cwv  (earth  coodinates)  */ 
time   to   limit,  /*  Time  to  reach  the  cwv  limits  */ 
bfhj7!,  /*  Selected  foothold  in  body  coordinates  */ 

bd   footposj7j:     *  Desired  foot  position  in  body  coordinates  */ 

static  vector  d   footpos!7|;  /*  Desired  foot  position  in  earth  coordinates  */ 
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foot    trajectory  * 
i  =  leg    number; 

if  ("selected    gait    =    =  FTL    GAIT) 

{ 

if  (transfer   flagi!  !=  ON) 

{ 

transfer    flag  ij  =  ON; 
liftoffflagii!  =  OFF; 
place   flag j i    =  OFF; 

/*  Save  foothold  position.  */ 
oldfhji x  =  fh  ij.x; 
oldfh  i   y  =  fh  i].y; 
oldfhji  .z  =  fhji|.z; 

projdist  =  LENGTH  *  0.21666 

switch  (i) 

\ 

case  1 : 


* 


HALFLENGTH; 


*  find  new  left  foothold 

vx=  trans   rate->x; 

vy  =  trans   rate->y  +  rot   rate->z 

rel    hd  =  atan2(vy.vx): 

bfh  1  .y  =  82.0; 

bfh  1    x  -  HALFLENGTH-proj   dist*cos(rel   hd)-(82.0- 
proj   dist*sin(rel   hd))*tan(rel   hd): 

bfh  1    z  =  -T60.0; 

/*  Transform  to  earth  coordinates.  */ 
'    fh  i]]T  =  h  *  [bfh  i]]T    */ 

transform    point (fh.h, bfh, 1); 

break: 
case  2:     /*  find  new  right  foothold  */ 

vx—  trans   rate->x; 

vy  =  trans   rate- >y  +  rot   rate- >z  *  HALFLENGTH; 

rel    hd  =  atan2(vy,vx); 

bfhT2;.y  =  -82.0; 

bfh|2<.x  =  HALFLENGTH-^proj  dist*cos(rel   hd)-(-82.0- 
proj   dist*sin(rel   hd))*tan(rel   hd); 

bfhl2|.z  =  -160.0; 

/*  Transform  to  earth  coordinates.  */ 

/*  [fh[i]]T  =  h  *  [bfh|i]]T   */ 

transform   point(fh.h.bfh.2); 

break: 
default:      *  back  leg  uses  old  front  leg  foothold  * 

fhiil.x  =  oldfh|i-2].x; 

fhjij.y  =  oldfh[i-2].y; 

fh  l  .z  =  oldfhii-2.z; 
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*    foot    trajectory  * 

/*  determine  the  desired  foot  position  */ 
d    footpos[i;.x  =  fhjij.x; 
d    footposji    y  =  fh[i].y; 
d    footposji  .z  =  fhli  .z; 


else  *  FWD    WAVE   GAIT    Calculate  a  new  desired  foot  position 

at  each  time  increment.  */ 

{ 

/*  Calculate  the  desired  touchdown  point    * 

/*  Future  change  note:    Change  from  cwv  center  to  midstance.  */ 

/*    Calculate  foot  velocity  at  center  of  cwv  (body  coordinates)  */ 
cwv    velocity. x  =  trans   rate->x  -  rot   rate->z  *    cwvjij.y. center 

+  rot    rate->y  *    cwv[i|.z. center; 
cwv   velocity. y  =  trans   rate->y  +  rot   rate->z  *    cwv  it. x. center 

-  rot    rate->x  *    cwv[i;.z. center: 
cwv    velocity. z  =  trans   rate-    z  -  rot    rate->y  *    cwv|i|.x. center 

4-  rot   rate->x  *    cwvji].y. center: 

/*  Calculate  the  time  to  reach  the  limits  of  the  cwv.  */ 
if  (cwv   velocity. x  <  0.0) 

time   to   limit. x  =  (  cwv  ij.x.min  -    cwv  1  .x. center) /cwv   velocity. x; 

se  if  (cwv    velocity. x    >  0.0) 

time   to   limit. x  =  (  cwviij.x.max  -    cwv[i].x.center)/cwv   velocity. x; 

else 

timetolimit.x  =  LONGTIME; 

(cwv    velocity. y  <  0.0) 

time  to  limit. y  =  (  cwv|il.y.min  -  cwvfij.y. center)  /cwv  velocity. y; 
Ise  if  (cwv    velocity. y  >  0.0) 

time   to    limit. y  =  (  cwv  ij.y.max  -    cwv'il.y. center)   cwv    velocity. y; 

lse 

time   to   limit.v  -  LONGTIME; 
} 
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;    foot    trajectory  * 

if  (cwv    \elocitv.z  <  0.0) 


time   to    limit. z  =  (  cwv[i].z.min  -    cwvji    z. center)   cwv    velocity. z: 

} 

else  if  (cwv    velocity. z  >  0.0) 

{ 

time   to   limit. z  =  (  cwvji]. z. max  -    cwv[i.z. center)   cwv   velocity. z; 

} 
else 

{ 
time  to   limit. z  =  LONG   TIME: 

} 

Determine  the  minimum  time  to  reach  the  cwv  limit.  */ 
min    time  =  time   to   limit. x; 
if  (time   to   limit. y  <  min   time) 

{ 

min   time  =  time   to   limit. y: 

} 

if  (time    to    limit. z  <  min    time) 

{ 

min   time  =  time   to   limit. z; 

I 

*  Calculate  the  desired  touchdown  point  in  body  coordinates.  */ 

*  Note:    This  point  changes  if  the  body  is  in  motion.  */ 

bd  footpos  1  .x  =  cwv[i|.x. center  -+-  cwv  velocity. x  *  min  time  *  .9; 
bd  footpos  i  y  =  cwv[i].y. center  -r  cwv  velocity. y  *  min  time  *  .9; 
bd    footpos  i  .z  =    cwvji    z. center  —  cwv   velocity. z  *  min    time  *  .9: 

*  Transform  to  Earth  coodinates.    */ 

/*     id   footpos  i   T  =  h  *  ;bd   footposjijlT    */ 
transform    point (d   footpos,h,bd   footpos, i); 


I 


Calculate  the  time  remaining  in  the  transfer  forward  phase.  */ 
trans   fwd    time  =  *trans    time  *  (*begin    place   phase  -  *trans    phase); 

Calculate  the  new  foot  position.  (Earth  Coordinates)  */ 

if  (DELTA   TIME  <  trans  fwdtime) 

r 
t 

footposjij.x  +=  (d   footpos  i  .x  -    footpos|i;.x) 

*  DELTA   TIME  /  trans   fwd   time; 
footpos  ij.y  -*-=  (d    footpos  i!.y  -    footpos  i   y) 

*  DELT A   TIME  /  trans  fwd   time; 

footpos  i|.z  =     footpos; ij.z:  /*  Level  ground  assumption!  * 

I 
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;  *    foot    trajectory 

else  *    Last  increment  of  time  * 

{ 

footpos[i'.x  =  d   footpos[i].x: 

foot  post  iy  =  d    footpos[i).y; 

footpos|ii.z  =    footpos|ii.z;    /*  Level  ground  assumption!  */ 
} 

/*    Transform  to  body  coodinates.    */ 

/*     [b_footpos[i]]T  =  invh  *  jfootposjijlT    */' 

transform    point (b   footpos,invh,footpos,i); 

}      /*  end  of  transfer   trajectory    */ 
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*  foot    ( rajectory  *. 

placement    trajectory(liftoff  flag. place    flag. transfer   flag.footpos. 
b   foot pos.invh. trans   time. trans   phase,  leg    number) 

*  This  function  calculates  the  trajectory  of  the  foot  while  it  is 

being  lowered  from  the  ground.    It  is  called  from  foot    trajectory().*/ 

vector      footpos|7],        *    Present  foot  position  in  earth  coordinates  */ 
b   footpos|7];  /*    Present  foot  position  in  body  coordinates    */ 

float       *trans   time, 
*trans   phase, 
invh  4  [4  :         *  Inverse  homogeneous  transformation  matrix  */ 

int  liftoff  flag!?:,  /*  Indicates  the  first  time  entering  subroutine 

in  current  leg  cycle  */ 
transfer    flag  7], 
place    flag  7  . 
leg    number: 

{ 

float      place    time: 

int         i: 

static  vector     d    footpos|7|;  /*  Desired  foot  position  in  earth  coordinates  */ 

i  —  leg    number: 

Calculate  the  desired  foot  position.  *  ' 
if  (  place_flagjii  !=  ON) 

{ 

d   footposjij.z  =    footposji;. z  -  FOOTLIFTHEIGHT; 

liftoff-  flagji:  =  OFF; 

transferflag'i]  =  OFF; 

place   flag  i1  =  ON: 
} 

/*    Calculate  the  time  required  to  reach  the  desired  height 

from  the  present  foot  position.    */ 
place   time  =  'trans   time  *  (1.0-  *trans    phase); 

/*    Calculate  the  new  foot  position.  (Earth  Coordinates)  */ 
if  (DELTA   TIME  <  place  time) 

{ 

footposji  .z  +=  (d   footpos  i  .z  -    footposji  .z) 
*  DELTA   TIME     placetime: 

I 
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,'*    foot    trajectory*/ 

else  Last  increment  of  time  */' 

{ 

footpos  i    z  =  d   footpos|ij.z: 

} 

/*    Transform  to  body  coodinates.    */ 

/*     [b_footpos|i]]T  =  invh  *  [footpos[i]]T    */ 

transform    point (b   footpos, invh, footpos, i); 

}      /*  end  of  placement    trajectory    */ 
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foot    trajectory 


/ 


support    trajectory  (lift  off  flag, place    flag, transfer   flag, footpos. 
b   footpos. invh, leg   number) 


This  function  calculates  the  trajectory  of  the  foot  during  the 


foot  support  phase.    It  is  called  from  foot    trajectory ()  * 


vector      footpos  7],     /*    Present  foot  position  in  earth  coordinates  * 
b   footpos: 7];    '      Present  foot  position  in  body  coordinates    */ 

float         invh[4j[4  ;        /x  Inverse  homogeneous  transformation  matrix  * 

int  liftoff  flag  7  ,     *  Indicates  the  first  time  entering  subroutine 

in  current  leg  cycle  *  l 
transfer   flag7j, 
place    Hag  7  . 
leg    number: 


int        i: 


In  this  phase  the  foot  is  kept  stationary  on  the  ground.    It 
is  assumed  that  the  foot  will  not  slip  or  move  accidently.  */ 

i  =  leg    number. 

Transform  foot  position  to  body  coodinates.    *  j 
b   footpos  i   T  =  invh  *    footpos  i   T 
transform    pointfb   footpos. invh. footpos. i); 

*    Turn  off  flags.  */ 
liftoffflag  i    =  OFF; 
transfer   flag  i    =  OFF; 
place    flagji!  =  OFF; 

}      /*  end  of  support   trajectory    */ 
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********XX******xx%***XX*******  +  xx.  **lfLX**xx±x**x*;xx  +  lf********X*:****X 


This  function  is  for  the  program  walk.c  on  the  iris-2400. 
conwalk.c 


Based  in  part  on  J.H.Kesslers 
R.  L.  Lyman  program  "conwalker.c" 

24  Apr  1987 

#include  "gl.h" 
^include  "device. h" 
^include  "walk.h" 

This  function  calls  up  the  walker  from  constructwalker  (with  legs 
already  properly  positioned)  and  then  rotates  and  translates  it  as 
commanded. 

Note:    Due  to  the  limited  number  of  bit  planes  available 
four  separate  walkers  are  constructed,  one  for  each  viewing 
quadrant.    The  walker  for  each  quadrant  is  drawn  from  furthest 
component  to  nearest.    This  provides  a  quasi-  Z  buffer  effect 
while  in  double  buffer  mode.  */ 


m  akew  alker(machineobject,dl,d2,theta.  knee,  gam  ma,  alpha,  transrot    tag. 
tr   end    tag. walker, leg. thighobj.actuatorobj.shinobj, 
legmovetag,thighmovetag.actmovetag,shinmovetag  ,tx,ty,tz, 
roll,elev.azimuth.hx.hy,hz.l4) 

Tag       transrot    tag;4  .tr  end   tag|4{,legmovetag  )  41, 

thighmovetag[]j2J|4],actmovetag[]|2i|4|,shinmovetagj][2!|4]; 

Object  machineobject!4:.walker[4],thighobJi][2][4],actuatorobjij[2]!4J, 

shinobji]i2);4],leg[][4J; 

int  dl    ,d2jj,knee[][2]  ; 

Angle  thetaj  , alpha   ,gammaj]; 

float  tx,ty,tz, roll, azimuth, elev, 

hx[7],hy[7],hz[7],l4[7]; 


int  n; 


/ 
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'*  conwalk.c  * 

eonstructwalker(walker.dl,d2.knee.alpha.gamma.theta.leg,thighobj, 
actuatorobj.shinobj.legmovetag.thighmovetag.actmovetag. 
shinmovetag,hx.hy,hz,l4)  ; 

for  (n=0;  n<4:  n^ — )         *  Rotate  and  translate  the  walkers  in  each 
quadrant.    */ 

{ 

machineobject!n]=genobj(); 

makeobj(machineobjectinj); 
pushmatrixf)  ; 

/*  Note:  Each  walker  is  built  on  the  origin.  Rotations  are  done 
before  translating  to  the  proper  location.  */ 
transrot    tag  n  =gentag(): 
rnaketag(transrot    tag  n  ); 

translate(tx,ty,tz); 

rotate(jint)  (elev  *  573), 'Y'); 
rotate((int)  (roll  *  573). 'X'); 
rotate((int)  (azimuth  *  573), 'Z'); 

tr   end    tag  n  =gentag(): 
maketag(tr   end    tag  n  ); 

callobj(walkerjni); 

popmatnx()  : 
closeobj( ); 
}     *  end  quadrant  loop    */ 
}      '*  end  of  makewalker     * 
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/*  conwalk.c  * 

makeground(groundobject ,) 

/*    This  function  creates  a  checkerboard  groundplane  below  the  ASV  object.*  ' 

Object    *groundobject: 

{ 

Object    squareobject; 
Tag        transqrtag; 

static  int 

groundl[4]|3j  =  {{  1000.-500,0}, {1000,500.0}, {-1000,500,0},  {-1000,-500.0}}, 
ground2[4][3]  =  {{  2000,-1000,0},  {2000, 1000,0},  {-2000, 1000,0}.  {-2000,-1000,0}}, 
square[4'j(3]  =  {  {0,-100,0}, {0,0,0},  {-100. 0.0},  {-100, -100,0}}; 

int        i,j; 

float     tx,ty; 

squareobject  ^genobj(); 
makeobj  (squareobject ); 
colorf  WHITE); 
polfi(4, square); 

closeobj( ): 

*groundobject  =  genobj( ); 
makeobj  (*groundobject); 

color(RED);  /*    fill  outer  background  squares    */ 

polfi(4,ground2); 

color(GREEN);         /*    fill  inner  background  squares    */ 
polfi(4,groundl); 

for  (i=0;  i<40:  i++) 

{ 
for  (j  =  0;  j<20;  j  +  +  ) 

{ 
if  ((i+j)%2<  1) 

{ 
tx=(i-20)*(-100.0); 
ty=(j- 10)*  (-100.0); 

pushmatrix(); 
translate  (tx,ty, 0.0); 

callobj(squareobject);     '*  place  the  white  squares  */ 
popmatrixf); 
}   /*  end  if*/ 
}  /*  end  for  j  */ 
}    /*  end  for  i    */ 
closeobjj): 
}    /*    end  makeground  * / 
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/  *  conwalk.c 

const ructw  alker(w  alker,dl.d2.knee.alpha.gamma.theta,leg.thighobj, 
actuatorobj,shinobj.legmovetag.thighmovetag.actmovetag, 
shinmovetag.hx.hy.hz.U) 

/*     This  is  where  the  walker  is  made.  Here  each  part  is  assembled 
and  then  the  parts  are  put  together.  This  assembled  walker  is 
then  rotated  and  translated  in  rnakewalker  which  is  called  by 
the  main  program.  */ 

Tag  legmovetag[;[4    .thighmovetag;ji2][4J,actmovetag[)[2][4l, 
shmmovetag  ]|2][4j; 

Object  walker|4  .leg  '[4  .thighobj[](2][4].actuatorobj[;[2][4J,shinobj[][2][4i; 

int  dl    .d2    .knee  j!2]; 

Angle  alpha|    gamma[!.theta    ; 

float  hx|7],hy[7],hz[7j,  /*  leg  pivot  position  *  ' 

1417  : 

{ 
Object  body. head. eye, boxobj[7J  : 

static  float  leg.x  7!  =  {0. 0.155. 0.155. 0,0. 0.0. 0,-155. 0,-155.0}, 
legy  7  ={0.0.82  0.-82.0.82.0,-82.0,82.0,-82.0}, 
legziT  -{0.0.0.0,0.0,0.0,0.0,0.0,0.0}; 

Coord  x,y.z  ; 

int  i,j.k.n,legnum  ; 

static  int 

/*  Coordinates  for  building  the  body  of  the  asv  */ 

blackbody[4][3j={  {206, 50,22},  {206,-50, 22},  {206.-30,-101},  {206, 30.-101}}, 
lbody  arry  [4]  j3]  =  {  {-200,30,-101},  {-200. 50. 22},  {206,50, 22},  {206. 30,-101}}, 
rbodyarryi4J[3j  =  {  {-200,-30,-101},  {206,-30,-101}.  {206.-50,22},  {-200,-50,22}}, 
tbodyarry[4][3j  =  {  {-200, 50,22}, {-200,-50, 22}. {206,-50, 22},  {206, 50,22}}, 
bbodyarry(4|j3!  =  {{  206,-30,-101}, {-200.-30.-101}, {-200, 30.-101}, {206. 30, -101}}, 
backbodyarry(4l3]  =  {  {-200, 30,-101}, {-200. -30,-101}, {-200.-50. 22}, {-200, 50, 22}}, 
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/*  conwalk.c 

/*  Coordinates  for  building  the  hydraulics  housing  structure  */ 

front _rt_topi4]|3]  =  {  {27,-25. 16},  {38.-25.-13},  {.38,  -13,-13},  {27.- 13, 16}}, 

front  _rt_bttm|4]|3!  =  {  {38,-25.- 13},  {38.-25.-46}. {38,-13.-46}, {38,- 13. -13}}, 

rt    interior  5jj3|  =  {  {20,-25, 38}, {38,-25,- 13}, {38.-25.-46}. {-38.-25.-46}, {-38,-25, 38} 

rt_side[5i[S]  =  {  {-38,-25.38}, {-38,-25,-46}, {38,-25,-46}, {38.-25.- 13}.  {20,-25,38}}, 

lt_intenor:5    3  =  {{-38, 25, 38}, {-38, 25.-46}. {38, 25,-46}, {38, 25,-13}, {20, 25, 38 } }, 

lt_side[5j[3]  =  {{20,25,38}, {38,25,-13}, {38,25,-46}, {-38,25,-46}, {-38,25,38}}, 

top_box[4l[3i  =  {{20,-25,38},{20,25,38},{-38,25,38},{-38,-25.38,}}, 

back_box[4![3i  =  {  {-38, 25,-46}.  {-38,-25,-46}, {-38,-25, 38}, {-38, 25, 38}}. 

front  top[4][3j={{20,25,38}, {20,-25, 38}, {27,-25, 13}, {27,25.13}}, 

front  Jt_top|4J|3]  =  {  {27, 13, 16}, {38. 13.-13} ,{38,25,-13}. {27,25. 16}}, 

front_lt_bttm[4][3]  =  {{38,13.-13}, {38, 13.-46}, {38,25.-46},{38,25.-13}}, 

bttm_lt[4][3]  =  {  {38. 25,-46},  {38, 13,-46},  {-38, 13,-46},  {-38,25,-46}}, 

bttm_rt[4j;3]  =  {  {38.-25, -46}, {38, -13,-46}, {-38, -13, -46}, {-38.-25, -46}}. 

highbox_topi4][3]  =  {{-8,-25,88},{8,-25,88},{8,25,88},{-8.25.88}}. 
highbox_front(4][3]  =  {{8,25,88},{8,-25,88},{l0,-25,38},{l0,25,38}}, 
highbox_back[4][3]  =  {  {-8,-25,88}, {-8.25,88}, {-10,25,38}, {-10,-25.38}}, 
highbox_rt[4)[3)  =  {{  8,-25, 88},  {-8,-25, 88},  {-10,-25, 38},  {10,-25, 38}}, 
highbox  lt[4][3]  =  { {-8,25,88}, {8,25,88}, {10,25,38}, {-10,25,38}}, 

rtspar  front  14  |3  '  =  {  {79.-13, -20}.{ 79. -25.-20}, {  79.-25,-30}, {79,-13,-30} }, 
rtspartop  4    3>  =  {{  79.  -13.-20},  {38.- 13.- 19}.  {38,  -25,-19},  {79,-25,-20}}, 
rtspar   bttm  4  [3  ={  {38.-13,-32}  .{79. -13.-30}, {  79. -25,-30}, {38,-25,-32}}, 
rt_spar_rt<4    3  -{{38,-25.-32}, {79,-25. -30}. {79. -25,-20}, {38,-25,- 19}}. 
rt_spar_lt;4!  3i  =  {  {79. -13,-30}, {38,-1 3,-32}. {38.- 13.- 19}, {79,-13,-20} }, 

It   spar   front i4]|3j  =  {{ 79. 25, -20}, {79, 13, -20}, {79. 13, -30}. {79, 25.-30}}, 
It   spar JopJ4]|3]  =  { {79,25,-20} ,{38, 25,-19}, {38, 13,-19}, {79, 13,-20}}, 
lt_spar_bttm[4][3j  =  {  {38. 25,-32}, {79, 25,-30}, {79, 13,-30},  {38, 13,-32}}, 
lt_spar_rt'4][3}  =  {  {38, 13,-32}, {79. 13.-30}, {79, 13.-20}, {38, 13, -19}}, 
lt_spar_lt(4!j3]  =  {  {79,25,-30},  {38,25,-32},  {38, 25,-19},  {79,25,-20}}, 

/*  cab  construction  arrays  */ 

cab_bottom[4][3j  =  {  {305,-30,-101},  {206,-30,-101 },  {206,30,-101},  {305, 30,-101}}, 

cab  Jop;4;!3={  {250. 33. 74}, {206. 33. 74}. {206.-33, 74}, {250.-33. 74}}, 

cab  _fwd_support[4][3]  =  {  {305, 30,-101},  {305. 41,  -16},  {305.  -41,  -16},  {305,-30,  -101}}, 
cab_fwd_lower!4]|S]  =  {  {305. 41. -16}, {318,48.8}  ,{318,-48. 8}, {305.-41. -16}}, 
cab_fwd_upper:4j|3]  =  {{318,48,8},{302,33.68},{302.-33.68},{318.-48,8}}, 
cab_fwd_ovhd[4j|3  ={  {275, 33, 68}, {250. 33.74 },{  250, -33, 74}. {275, -33,68} }, 

cab_rt_support[4][3]={{305,-30,-10l}, {305,-41,-16}, {206,-41.-16}, {206,-30,-101}}, 

cab   rt_lower|4)[3!  =  {{305,-4 1,-16}, {318.-48, 8}, {206. -48. 8}, {206,-41. -16}}, 
cab_rt_upperJ4]!3]  =  {{318,-48,8},{302,-33,68}.{206,-33,68},{206,-48,8}}. 
cabj-t_ovhd[4][3]  =  {  {275,-33,68},  {250.-33, 74},  {206.-33, 74},  {206.-33.68}}, 
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/*  conwalk.c  * 

cabjt    support  4    3      { {206.30.-101  [,{206. 41,  -16},  {305. 41  .-16},  {305. 30.  -101 } }. 
cab   It    lowerj4:[3 >{  {206. 4 1.-16}. {206. 48, 8}, {318. 48.8}, {  305.41,-16} }, 
cabltupper  4    3  =  {{  206. 48. 8},  {206. 33,68},  {302. 33,68},  {318. 48.8} }, 
cab_lt_ovhd  4  '3j  =  {  {206, 33,68}, {206,33, 74}, {250, 33. 74}.{  275,33,68} }, 

cabaft   supports    3  '.  =  {  {206.  -30,  -101},  {  206,-41,  -16}.  {206. 41.  -16},  {206, 30,  -101} }, 
cabaft  _lower|4i  [3;  =  {{206.-41.- 16}.  {206,-48,8}. {206,48,8}, {206,41. -16}}, 
cabaft    upper, 4|[3j  =  {{ 206,-48. 8}. {206.-33. 68}, {206,33. 68}. {206.48, 8}}, 
cab_aft_ovhd[4][3]  =  {  {206,-33,68}, {206,-33, 74}, {206, 33, 74}, {206,33, 68}}. 

scanner  fwdlower!4]3]  =  { {302. 33, 68}, {322, 33, 95}, {322,-33, 95}, {302, -33. 68}}, 
scanner_fwd_upper[4][3]  =  {{322, 33, 95}, {322,33,101}, {322,-33, 101}, {322,-33,95}}, 
scanner  _rtj5]|3j  =  {  {302,-33,68},  {322,-33. 95}. {322,-33, 101}, {275,-33. 101}, {275,-33.68}}. 
scanner  lt[5][3]={{302,33,68},{275,33,68}, {275,33, 101}, {322,33, 101}, {322,33,95}}, 
scanner_aft[4][3]  =  {{275, 33, 101}, {275, 33,68}, {275,-33,68}, {275,-33, 101}}, 
scanner   top  4:  3i  =  {{322. 33, 101}. {275, 33. 101}. {275,-33, 101}, {322,-33. 101}}; 

The  making  of  the  leg  is  quite  complicated.  Each  leg  consists  of  an 
upper  link  (thigh),  lower  link  (actuator),  and  a  shank  (shin).  These 
segments  are  first  defined  in  a  standardized  orientation,  and  are  then 
rotated  and  translated  into  the  proper  position.  This  is  done  by  using 
2  objects  for  each  segment.  The  first  object  is  the  correctly  rotated 
segment,  and  the  second  object  is  the  correctly  translated  first 
object.  Thus  the  segment  is  then  in  the  proper  position.  To  hold  the 
screen  coordinate  system  fixed  the  matrix  is  pushed  before  each  translation 
or  rotation  and  then  popped  after  the  object  is  constructed  or  called.  */ 

for  (n=0;  n<4;  n  +  +  )      /*    Make  a  set  of  legs  for  each  viewing  quadrant 

k  / 
/ 


Each  quadrant  must  have  unique  tags. 


{ 


for(legnum  =  l  ;legnum<7;legnum-<--l-) 

{ 

/*  Each  segment  is  constructed  and  positioned    */ 
build thigh(n,legnum,dl, alpha, thighobj,thighmovetag)  ; 
build actuator(n,legnum,d2, alpha, actuatorobj,actmovetag)  ; 
buildsh in (n.legnum, knee, gamma, shinobj,shinmovetag)  ; 

leg  legnum  n,=genobj(); 
makeobj(leg  legnumjjn]); 
pushmatrix(); 

/*  translate(legx|legnum|,legy!legnum  .legz  legnum  )  ;  */ 

translate(hx|legnum  ,hylegnum  .hz  legnum])  ; 

legmovetag  legnum!ni  =  gentag();    /*  The  leg  is  assembled  from    */ 
maketag(!egmovetag;legnumj!n]);    /*  its  parts  and  the  entire  leg  is  * 

/*  then  rotated  to  the  proper  angle.  * 
rotatejthetailegnum  ,'X'); 
translate(0.0.l4|legnum:.0.0);  /*  extend  leg  outward  * 
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conv\  alk.c  *  / 

if  (((n  >  l)&&(legnum  <  5))|| 

((n  <  2)&&(legnum  >  4)))        /*  Build  the  left  side  first.  */ 

1 
if  (legnum  >  4)  *  Reverse  the  back  legs.  */ 

{ 

pushmatri.x) ); 
rotate(1800."Z'); 

} 

color(BLACK): 
polfi(5,lt    interior); 

color(GREEN): 
polfi(5,lt   side); 

polfi(4. front    It    top); 

polfi(4. front    It    bttm): 
polfi(4,bttmJt); 

polfi(4,lt  spar  front); 

polfi(4,lt  spar  bttm); 

polfi(4,lt  spar  It); 

polfi(4,lt  spar  rt); 

color(BLUE); 
polfi(4,lt   spar   top): 

color(BLACK); 

polyi(4,lt    spar   rt); 

color(CYAN)  ; 

callobjfthighobj'legnum    l]  nj); 
callobj(actuatorobj  legnum  [ l] [n i ) ; 
callobj(shinobj  legnum] jljjnj); 

color(GREEN)  ; 
polfi(4,rt    spar   front); 
polfi(4,rt    spar   bttm); 
polh(4,rt   spar   It); 
polfi(4.rt    spar   rt); 

color(BLUE); 
polfi(4,rt   spar   top); 

color(GREEN); 

polfi(4, front   rt   bttm); 
polfi(4, front    rt    top); 
polfi(4, front    top); 
polfi(4,bttm_rt); 
polfi(4,back    box); 
polfi(4.top   box); 
polfi(5,rt    side); 
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*  conwalk.c  * 


color(BLACK); 
polyi(4.top   box); 
polyi(5.rt    side); 
polyi(4.rt    spar   rt); 

color(GREEN); 

polfi(4,highbox    front); 
polfi(4,highbox    It); 
polfi(4,highbox    back); 
polfi(4,highbox    rt); 
polfi(4.highbox    top); 

color(BLACK); 

polyi(4.highbox   top); 
polyi(4.highbox   rt); 
polyi(4.highbox    back): 

if  (legnum   >  4)        /*  For  reversing  the  back  legs.  */ 

{ 

poprnatrixf)  ; 


else  *  Build  the  right  side  first.  */ 

{ 
if  (legnum       4)  *  Reverse  the  back  legs.  */ 

{ 

pushmatrixj ): 
rotate(1800.'Z); 


color(BLACK); 
polfi(5.rt    interior), 

color(GREEN); 
polfi(5,rt   side); 

polh(4.front_rt_top); 

polfi(4, front    rt    bttm); 
polfi(4,bttm_rt); 

polfi(4.rt   spar   bttm); 
polfi(4,rt_spar    rt); 
pcilfi  (4  .rt   sparlt); 
polfi(4.rt    spar   front); 

color(BLUE); 
polti(4,rt    spar   top); 


160 


conwalk.c  * 


color(BLACK); 

polyi(4.rt    spar   It); 

color(CYAN)  ; 

callobj(thighobjlegnum][l]inj); 
cal]obj(actuatorobj[legnum]j  l][nj 
callobj(shinobjjlegnum![lJ|n]); 

color(GREEN)  : 
polfi(4,back    box); 
polfi(4,bttm_lt); 

polfi(4. front    top); 
polfi  (4, front  _lt_bttm); 
polfi(4. front   It   top); 

polfi(4.1t  spar  bttm): 

polfi(4.1t  spar  rt); 

polfi(4.1t  spar  It); 

polfi (4 . It  spar  front); 

color(BLUE); 

polfi(4.1t    spar   top); 

color(GREEN); 

polfi(4.top    box); 
polfi ( 5,lt    side); 

color(BLACK); 
polyi(4,top   box); 
polyi(5,lt_side); 
polyi(4.1t    spar   It); 

color(GREEN); 

polfi(4.highbox    back); 
polfi(4.highbox    rt); 
polfi(4.highbox    front); 
polfi(4.highbox   It); 
polfi(4,highbox   top); 

color(BLACK); 
polyi(4,highbox   top); 
polyi(4,highbox   It); 
polyi(4,highbox   front); 
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*  con  walk,  c 

if  (legnum    >  4)  *  For  reversing  the  back  legs.  * 

{ 

popmatrix()  ; 


popmatrix(); 
closeobj()  : 
}    /*  end  of  leg  loop  */ 
}    /*  end  of  quadrant  loop  *  ' 

body  =  genobj()  : 
makeobj(body); 

color(LTYELLOVV)  ; 
polfi(4.lbodyarry)  ; 
polfi(4,backbodyarry)  ; 
polfi(4.bbodyarry )  ; 
polfi(4,rbodyarry)  ; 

color(  YELLOW); 

polfi(4.tbodyarry); 

color( BLACK)  ; 
polfi(  4.  black  body)  : 
closeobj()  ; 

head  =  genobj()  : 
makeobj(head)  : 

color(  YELLOW); 

polfi(4,cab    top); 

polfi(4.cab   fwd    ovhd); 

polfi(4,cab   rt    ovhd); 

po!fi(4,cab    It    ovhd); 

polfi(4.cab    aft   ovhd); 

color(BLACK); 
polfi(4,cab   bottom); 
polti(4,cab   fwd    support); 
polti(4.cab   rt    support); 
polh(4,cab   It   support); 
polfi(4.cab    aft   support); 

color)  WHITEl); 
polti(4,cab   fwd   lower); 
polfi(4,cab   rt    lower); 
polh(4.cab    It    lower); 
polfi(4,cab   aft   lower): 


/*  The  body  is  constructed  */ 


/*  construct  the  head  */ 
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*  conwalk.c 

color(WHITE)  : 

polfi(4.cab   fwd    upper); 
polfi(4,cab   rt    upper): 
polfi(4,cab    It    upper); 
polfi(4,cab   aft    upper); 

color(BLACK)  ; 

polfi(4,cab   top): 
polyi(4.cab   fwd    lower); 
polyi(4.cab   fwd    upper); 
polyi(4.cab   fwd   ovhd); 
polyi(4.cab   rt   lower); 
polyi(4,cab   rt   upper); 
polyi(4.cab   rt    ovhd); 
polyi(4,cab    It    lower): 
polyi(4,cab   It    upper); 
polyi(4,cab   It   ovhd); 
closeobj()  ; 

eye=genobj()  :  /*  contruct  the  radar  (eye)*/ 

makeobj(eye)  : 

oolor(RED): 

polfi(4. scanner   fwd    upper); 

polfi(4. scanner   fwd    lower); 

polfi(5, scanner   rt): 

polfi(5, scanner   It); 

polfi(4, scanner   aft); 

color(BLACK)  ; 
polfi(4. scanner   top); 

color(BLUE)  ; 
closeobj()  : 

walker  jO!  =  genobj();  /*    assemble  all  the  parts  for  quad  I  */' 

makeobj(walkerjOj);  /*  back  and  right  first  */ 

callobj(legl6:i0  ); 

callobj(legj4jjo  ); 

caUobj(leg[2][0j); 

callobj(body); 

callobj(head); 

callobj(eye); 

callobj(leg[5jj0  ); 

callobj(leg[3  10  ); 

callobj(legjl'|0  ); 
closeobj()  ; 
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walker  1  -genobjf 
makeobjfwalker  1 
callobjfleg  2)[l]j 
callobjfleg  4    1  ) 
callobjfleg  6][lj) 
callobj(head): 
callobj(eye); 
callobj(bod>  ): 
callobjfleg  ljjli) 
callobj(leg  3][l|) 
callobjfleg  olfll) 


assemble  all  the  parts  for  quad  II 
'    front  and  right  first  * 


walker  2  =  genobj( 
makeobj(walker;2j 
callobjfleg  l](2j) 
callobj(legi3][2j) 
callobjfleg  5]'2;) 
callobjfhead); 
callobjfeye): 
callobjfbodv): 
callobjfleg  2|  2  ) 


callobjfleg 


4][2]) 


callobjfleg  6|'2 


assemble  all  the  parts  for  quad  III* 
'    front  and  left  first  * 


walker|3|  =  genobjf 
makeobjfwalker  3 
callobjfleg  5][3j) 
callobjfleg  3    3 
callobjfleg  l][3 
callobjfbodv): 
callobjfhead): 
callobjfeye); 
callobjfleg  6]  3 
cal!obj(legi4][3] 
callobj(leg!2]l3] 


assemble  all  the  parts  for  quad  IV  */ 
back  and  left  first  */ 
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/*  conwalk.c  * 

buildthigh(n,legnum.dl.alpha.thighobj  .thighmovetag) 

/*    this  function  constructs  the  thigh  (upper  link)  and  rotates,  then 
translates  it  into  the  proper  position  */ 

Tag  thighmovetag'j[2][4]; 
int  n,legnum.dl[j  ; 
Angle  alpha}]  ; 
Object  thighobj[][2][4j; 

{ 
static  int 

thighltside[4]}3  :  =  {{0.10, 7},{  102, 10, 7},  {102,10.-7  }.{  0.10,-7}}, 
thighrtside[4](3j  =  {  {0,-10,-7}, {102.- 10.-7}, {102.- 10,7}, {0.-10,7}}. 
thighfrontJ4j[3]  =  { {0.-10.7}. {102.-10,7}, {102. 10.7}, {0,10,7}}  , 
thighbttmj4][3]  =  {{0, 10,-7},  { 102,10.-7},  {102,-10.-7},  {0.-10,-7}}; 


thighobjjlegnumi(0![n]  =  genobj(); 
makeobj(thighobj'legnum  ][0]  jn]); 

pushmatrixf)  : 

thighmovetag|legnum    0][n]=gentag();       /*    rotate  thigh    */ 
maketagf thighmovetag  legnum;;0j|n|); 
roLatefalphadegnum'.'Y)  ; 

if(legnum    4)        *  Build  the  left  side  first.  */ 

{ 
color(CYAN); 

polfi(4.thighbttm); 
polfi(4,thighltside); 
polfi(4,thighrtside); 

color(RED); 
polfi(4,thighfront); 

color(BLACK); 
polyi(4,thighrtside); 
} 
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else  *  Build  the  right  side  first.  */ 

{ 
color(CYAN); 

polfi(4,thighbttm); 

polfi(4.thighrtside); 

polfi(4,thighltside); 

color(RED); 
polfi(4,thighfront); 

color(BLACK); 
pt>lv  i(4.t  highlt side) : 


popmatrixlj  ; 

closeobjf)  : 

thighobjjlegnum    1    n  =  genobj()  : 
makeobj(thighobj  legnumj  1  [nj)  ; 

pushmatrixf)  : 

t highmovetag  legnumj  1    n;  =  gentag(); 

maketag(thighmovetag  legnum)' ljjnj);  /*  translate  thigh    */ 

translate(0.n,0  0.( float  )(-dl[ legnumj))  ; 

callobj(thighohj  legnum,  0    n  ); 
popmatri\()  ; 

closeobj()  ; 
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/*  conwalk.c  */ 

/ 

build  act  u  at  or(n,legnum,d2.alpha,actuatorobj,actmovetag) 
/*  construct  the  actuator  (lower  link)  */ 

Tag  actmovetag[)j2][4]; 

int  n,legnum,d2j|; 

Angle  alphaj]; 

Object  actuatorobj[][2][4); 
{ 

static  int  act  ltside|4]  [3]  =  {{0,10, 5}, {83, 10, 5}, {83. 10, -5}.  {0,10, -5} }. 
actfront[4][3j={{0,-10,5}, {83,-10,5}, {83, 10,5}, {0,10,5}}  , 
actrtside[4][3]  =  {  {0.-10.-5 },  {83.-10,-5},  {83.  -10, 5},  {0,-10,5}}, 
actbttm|4]i3!  =  { {0,10,-5}, {83, 10, -5}, {83, -10, -5}. {0,-10,-5}}  ; 


actuatorobj  legnum]  0][n:=genobj(); 
rnakeobj(actuatorobj  legnum][0jln  ); 

pushmatrixf ); 

actmovetag  legnum]i0][nj  =  gentag(); 

maketag(actmovetag;legnum][0][n]);     /*    rotate  actuator  *  / 
rotate(alpha  legnumj,'Y')  ; 

if(legnum>4)      '*  Build  the  left  side  first.  */ 

{ 
color(CYAN); 

polfi(4.actbttm), 
polfi(4.actltside); 
polfi(4.actrtside); 

color(RED); 
polfi(4.actfront): 

color(BLACK); 

polvi(4.actrtside); 
} 
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*  conwalk.c  * 

else  /*  Build  the  right  side  first.  * 

{ 
color(CYAN); 

polfi(4,actbttm): 
polfi(4,actrtside); 
polfi(4.actltside): 

color(RED); 
polfi(4,actfront); 

color(BLACK); 
polvi(4,actltside); 

} 

popmatrix( ): 
closeobjf ); 

actuatorobjilegnum  | 1  [n  =genobj( ): 
makeobjfactuatorobjlegnum  |l  [n  ); 

pushmatrix(): 

actmovetag  legnum  [lj  n   -gentag(); 

maket  ag(aotmovetagdegnum]ll]lni);  /*  translate  actuator  */ 

translate!  (float  )(d2  1egnumj), 0.0, (float  )(-L3)); 
callobjfactuatorobj  legnum[0|lnl); 
popmatri.x( ); 
closeobj!); 
}     *  end  of  buildactuator  */ 
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*  conwalk.c  */ 

/  / 

buildshin(n,legnum,knee,gamma,shinobj.shinmovetag) 

/*    construct  the  shank  (shin  )  */ 

Tag  shinmovetagj][2]|4  : 
int  n,legnum,kneej|(2i; 
Angle  gamma]]; 
Object  shinobj[]:2!.l4]; 

\ 

static  int 

shinltside[6][3]={{6, 5,3}, {10,5,-59}, {-7, 5,-50}, {-6, 5, 3}, {-3,5,6}, {3,5,6}}, 

shankltside[4][3]={{l0,5,-59}, {-23, 5,-102}, {-36, 5,-100}, {-7, 5,-50}}  , 

shinfront  4][3  ={  {6, 5. 3}, {6,-5. 3}, { 10. -5,-59}, { 10,5.-59} }, 

shankfront[4][3]  =  {{l0,-5,-59}1{-23,-5,-102}, {-23, 5,-102}, {7, 5,-59}}, 

ankleltside[6][3]={{-23, 5,-102}, {3, 5,-153}, {2, 5,-157}, {-3, 5,-158}, {-6, 5,-158}, {-36,5,-100}}, 

shinrtside!6]S3i  =  {{3,-5,6},{-3.-5.6}.{-6,-5.3},-{-7.-5.-50},{l0,-5.-60},{6,-5,3}}, 

shankrtside[4][3]={{-7,-5,-50}, {-36,-5,-100}, {-23,-5,-102}, {10,-5,-59}}  , 

anklertside[6][3]  =  {{-36,-5,-100}, {-6,-5,-158}, {-3,-5,-158}, {2,-5,-157}. {3,-5,-153}. {-23,-5,-102}}, 

anklefront[4][3]  =  {{-23, 5,-102}, {-23,-5,-102}, {3,-5,-153}, {3, 5,-153}}, 

shinback[4][3]={{-7,-5,-50}, {-6,-5, 3}, {-6,5, 3}, {-7,5,-50}}, 

shankback[4][3]  =  {{-36,-5,-100},{-7,-5,-50},{-7, 5,-50}, {-36,5,-100}}, 

ankleback(4]|3]  =  {  {-6,-5,-158},  {-36,-5,-100},  {-36. 5,-100}.  {-6. 5.-158}}, 

bottom_fwd[4][3]={{3, 5,-153}, {2,  5,-157}, {2,-5,-157}, {3,-5,-153}}, 

bottom_mid[4][3]={{2,5,-157}, {-3,5,-158}, {-3,-5,-158}, {2,-5,-157}}, 

bottom   aft[4][3]={{-3,5,-158},{-6,5,-158}, {-6,-5,-158}, {-3,-5,-158}}; 

shinobjjlegnum][Oj[n|  =  genobj(); 
makeobj(shinobj|legnum][0]jn]); 
pushmatrix()  ; 

shinmovetag  legnum]|0Hn]=gentag(); 

maketag(shinmovetagjlegnum][0]jnj);       /*  rotate  shank  */ 
rot  ate  (gamma  legnum  ,'Y'); 

if(legnum>4)     /*  Build  the  left  side  first.  */ 


color(BLACK); 

polfi(4, bottom  fwd); 
polfi(4, bottom  mid); 
polfi (4, bottom    aft); 

color(CYAN); 
polfi(4,ankleback); 
polfi(6,ankleltside); 
polfi(6,anklertside); 

color(RED); 
polfi(4,anklefront)  ; 


169 
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color(CYAN); 

polfi(4.shankback)  ; 
polfi(4.shankltside): 
polfi(4.shankrtside); 

color(RED); 

polfi(4.shankfront )  ; 

color(CYAN); 

polfi(4.shinback); 

polfi(6.shinltside); 

polh(6.shinrtside); 

color(RED); 
polfi(4.shinfront)  ; 

color(BLACK); 

polyi(6.anklertside); 
polyi(4.shankrtside); 
poly  i(6.shinrt  side); 


Build  the  right  side  first.  */ 


color(BLACK); 
polfi(4. bottom    fwd) 
polfi(4. bottom   mid) 
polfi(4. bottom    aft)  ; 

color(CYAN); 

polfi(4.ankleback); 

polfi(6.anklertside); 

polfi(6.ankleltside); 

color(RED); 
polfi(4.anklefront); 

color(CYAN); 
polfi(4.shankback)  ; 
polfi(4,shankrtside); 
polfi(4.shankltside); 

color(RED); 
polfi(4.shankfront); 

color(CYAN); 
polfi(4.shinback); 
polfi(6.shinrtside); 
polfi(6.shinltside); 
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color(RED); 

polfi(4,shinfront): 

color(BLACK); 
polyi(6,ankleltside); 
poly  i(4,sh  an  kit  side); 
polyi(6,shinltside); 

} 
color(BLACK); 

pushmatrix(); 
rotate(-900,'X'); 

translatefO. 0,0. 0,5.0): 
circf(0. 0,0. 0,7.0)  ; 
circf(0. 0.32. 0.5.0)  ; 
popmatrix(); 

pushmatrix(); 
rotate(900.'X'); 

lranslate(0. 0,0. 0,5.0); 
circf(0.0.0.0.7.0)  : 
circf(0.0.-32. 0.5.0)  ; 
popmatrixf); 

popmatrix(); 
closeobj(); 

shinobj  legnum)[l][nj=genobj(); 
makeobj(shinobj[legnum|[lj(nj); 

pushmatrix(); 

shinmovetag[legnuml;  lj[n]=gentag(); 

maketag(shinmovetagjlegnumj(l][n]);  /*  translate  shank    */ 

translate ( (float )knee|legnumj ' 0],0.0,(float)knee  legnum][l))  ; 

callobj (shinobj  legnum)[0][nj); 

popmatrix(); 
closeobj(); 


}  /*  end  of  buildshin  */ 
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This  is  a  function  for  the  iris240()  program  walk.c. 
tool box. c 


Relle  Lvrnan 


25  Aug  1986 


/ 

include  "gl.h" 
^include  "device. h" 
^include  "walk.h" 
^include  •  stdio.h  • 
^include  <rnath.h  > 

transform    point (p2.m, pi. i) 

/*    This  function  changes  the  coordinate  system  for  a  point  vector 

using  a  homogeneous  transformation  submatrix.  p2  =  m  *  pi     */ 

ml       i;  /*  Leg  number  * 

float    m  4ji4j;  /*  Homogeneous  transformation  submatrix  */ 

vector  pi  7  .         *  Vector  represented  in  first  coordinate  system  */ 

p2[7l;       /*  Vector  represented  in  transformed  coordinate  system    */ 

{ 
P2  ii.x  =  m[0][0j*pl[i].x  +  m[0][l]*pl[i].y  +  m[0][2j*pl[ij.z  +  m[0][3l; 

P2  ij.y  =  m[l][0J*pl[i].x  -  m  1  j  1  i *pl[i  .y  +  m[l][2]*pl[i|.z  +  m[l][S]; 

p2  ii.z  =  m  2][0]*pl[i].x  +  m|2|[l]*plli].y  +  m[2][2]*pl[i].z  +  m[2)[3|; 

}  /  *  end  of  transform   point 

y****»******x*************»*T***x**********t>:***************>:*******X********/ 

/ 

float  modulus   one(temp) 

/*    This  function  performs  the  modulus  one  operation  on  numbers  of  type  float.  * j 

float  temp; 

{ 

while  (temp  >=  1.0) 

{ 

temp  -=  1.0; 

\ 

while  (temp  <  0.0) 

{ 

temp  +=  1.0; 

\ 

return  temp: 
}       *  end  of  modulus   one  */ 
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I*   Makefile  *, 

#  This  is  Makefile.    It  is  used  in  the  utility  make  to  speed 

#  compilation  of  walk. c.    To  use  it,  just  type  "make". 

CFLAGS  =  -Zf-Zg  -g 

SRCS  =        walk.c 
conwalk.c 
support. c 
toolbox. c 
steering. c 
body   rates. c 
ft    traj.c 
opt   period. c 
leg    phase. c 
con    work    vole 
driver. c 
status. c 
decelerate. c 
init.c 

OBJS  =        walk.o 
conwalk.o 
support. o 
toolbox. o 
steering. o 
body   rates. o 
ft   traj.o 
opt    period. o 
leg   phase. o 
con   work   vol.o 
driver. o 
status. o 
decelerate. o 
init.o 

walk  :     (OBJS) 

cc  -o  walk  (OBJS)    -Zg  -Zf 

(OBJS)  :        walk.h 
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